2024-11-06T04:35:18Z* LcmTrn::LcmTrn() version v1.0.3b0-518-g924cc45 built Oct 31 2024 16:19:57 - config file /home/tethysadmin/Downloads/trncfgs/lcm-trn-lrauv.cfg 2024-11-06T04:35:18Z* LcmTrn::init() - using configuration file /home/tethysadmin/Downloads/trncfgs/lcm-trn-lrauv.cfg 2024-11-06T04:35:18Z* ahrs config: AHRS_M2, platform_orientation, platform_pitch_angle, platform_roll_angle 2024-11-06T04:35:18Z* dvl config: RDI_Pathfinder, Beam1Range, Beam2Range, Beam3Range, Beam4Range, BottomVelocityFlag 2024-11-06T04:35:18Z* nav config: DeadReckonUsingMultipleVelocitySources, latitude, longitude 2024-11-06T04:35:18Z* depth config: Depth_Keller, depth, sea_water_pressure 2024-11-06T04:35:18Z* LCM timeout=0.20 sec 2024-11-06T04:35:18Z* TRN settings: 2024-11-06T04:35:18Z* period=4.00 sec 2024-11-06T04:35:18Z* coherence=0.90 sec 2024-11-06T04:35:18Z* map = PortTiles cfg = LRAUV_specs.cfg part= particles.cfg logdir= lrauv 2024-11-06T04:35:18Z* maptype = 2 filtertype = 2 weighting = 4 2024-11-06T04:35:18Z* lowgrade_filter = 0 allow reinit = 1 2024-11-06T04:35:18Z* cmd_estimate = estimate cmd_reinit = reinit 2024-11-06T04:35:18Z* LcmTrn::initTrn() version v1.0.3b0-518-g924cc45 - configuration file /home/tethysadmin/Downloads/trncfgs/lcm-trn-lrauv.cfg 2024-11-06T04:35:18Z* LcmTrn::initTrn() - map: /home/tethysadmin/Downloads/trnmaps/PortTiles 2024-11-06T04:35:18Z* LcmTrn::initTrn() - cfg: /home/tethysadmin/Downloads/trncfgs/LRAUV_specs.cfg 2024-11-06T04:35:18Z* LcmTrn::initTrn() - part: /home/tethysadmin/Downloads/trncfgs/particles.cfg 2024-11-06T04:35:18Z* TerrainMapOctree::tileDataName = /home/tethysadmin/Downloads/trnmaps/PortTiles/tiles.csv 2024-11-06T04:35:18Z* TerrainMapOctree::numTiles_ = 9. 2024-11-06T04:35:18Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile1.bo 4061250.0 594250.0 . 2024-11-06T04:35:18Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile2.bo 4061250.0 594750.0 . 2024-11-06T04:35:18Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile3.bo 4061250.0 595250.0 . 2024-11-06T04:35:18Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile4.bo 4061750.0 594250.0 . 2024-11-06T04:35:18Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile5.bo 4061750.0 594750.0 . 2024-11-06T04:35:18Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile6.bo 4061750.0 595250.0 . 2024-11-06T04:35:18Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile7.bo 4062250.0 594250.0 . 2024-11-06T04:35:18Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile8.bo 4062250.0 594750.0 . 2024-11-06T04:35:18Z* TerrainMapOctree::read tile line: /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile9.bo 4062250.0 595250.0 . 2024-11-06T04:35:18Z* TerrainMapOctree::loaded 9 useable tiles from /home/tethysadmin/Downloads/trnmaps/PortTiles/tiles.csv. 2024-11-06T04:35:18Z* TerrainMapOctree::Octree tile load /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile1.bo took 0.500015 seconds. 2024-11-06T04:35:19Z* session directory is /home/tethysadmin/Downloads/trnlogs/lrauv-TRN.14/ 2024-11-06T04:35:19Z* TRN log directory is /home/tethysadmin/Downloads/trnlogs/lrauv-TRN.14/ 2024-11-06T04:35:19Z* symlink /home/tethysadmin/Downloads/trnlogs/latestTRN to lrauv-TRN.14 OK 2024-11-06T04:35:19Z* parseSensorSpecs parsing sensor of type 1. 2024-11-06T04:35:19Z* Sensor 0 is of type 1. 2024-11-06T04:35:19Z- File created[/home/tethysadmin/Downloads/trnlogs/latestTRN/trn.log.0000] returned[0x5576920da0] 2024-11-06T04:35:19Z- TNavConfig::setMapFile: value is now /home/tethysadmin/Downloads/trnmaps/PortTiles 2024-11-06T04:35:19Z- TNavConfig::setVehicleSpecsFile: value is now /home/tethysadmin/Downloads/trncfgs/LRAUV_specs.cfg 2024-11-06T04:35:19Z- TNavConfig::setParticlesFile: value is now /home/tethysadmin/Downloads/trncfgs/particles.cfg 2024-11-06T04:35:19Z- TNavConfig::setLogDir: value is now /home/tethysadmin/Downloads/trnlogs/lrauv-TRN.14/ 2024-11-06T04:35:19Z- createFilter: Loading vehicle config file... /home/tethysadmin/Downloads/trncfgs/LRAUV_specs.cfg 2024-11-06T04:35:19Z- parseSensorSpecs parsing sensor of type 1. 2024-11-06T04:35:19Z- Sensor 0 is of type 1. 2024-11-06T04:35:19Z- Random noise generator initialized with 1730867719 2024-11-06T04:35:19Z- Vehicle name: LRAUV Number of sensors: 1 2024-11-06T04:35:19Z- Sensor #1: 2024-11-06T04:35:19Z- Sensor name: rdiDVL 2024-11-06T04:35:19Z- Sensor type: 1 2024-11-06T04:35:19Z- Number of beams per measurement: 4 2024-11-06T04:35:19Z- Sensor #1 to vehicle transformation information: 2024-11-06T04:35:19Z- Rotation angles (phi, theta, psi): (0.000000 ,0.000000, 0.000000) 2024-11-06T04:35:19Z- Translation vector [dx, dy, dz]: (0.000000 ,0.000000, 0.000000) 2024-11-06T04:35:19Z- 2024-11-06T04:35:19Z- setDistribToSave(0) 2024-11-06T04:35:19Z- setDistribToSave set to 0 2024-11-06T04:35:19Z- TerrainNav::TNavFilter initialized with type 2 2024-11-06T04:35:19Z- TerrainNav::initVariables finished. 2024-11-06T04:35:19Z- TerrainNav::Constructor finished. 2024-11-06T04:35:19Z- TerrainNav::modified weighting set to 4 2024-11-06T04:35:19Z- LcmTrn::initLcm() - configuration file /home/tethysadmin/Downloads/trncfgs/lcm-trn-lrauv.cfg 2024-11-06T04:35:19Z- LcmTrn::run() v1.0.3b0-518-g924cc45 using bursty timeouts of 50 and 150 msec 2024-11-06T04:35:19Z- AHRS_M2 msg: 1730867719.36 epoch sec; seqNo:5 2024-11-06T04:35:19Z- AHRS_M2 msg: 0.11 phi; -0.19 theta; 5.56 psi 2024-11-06T04:35:19Z- handleDepth()- Depth_Keller msg: 1730867719.37 epoch sec; seqNo:6; depth 2.19 2024-11-06T04:35:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867719.39 epoch sec; seqNo:6; 4061474.50 north; 594482.99 east; 36.69 lat; -121.94 lon; 2.19 depth; utmZone: 10 2024-11-06T04:35:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867719.39 2024-11-06T04:35:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:19Z- Waiting for fresh data: AHRS(1730867719.36), Dvl(-1.00), Nav(1730867719.39), Depth(1730867719.37) 2024-11-06T04:35:19Z- AHRS_M2 msg: 1730867719.76 epoch sec; seqNo:6 2024-11-06T04:35:19Z- AHRS_M2 msg: 0.10 phi; -0.18 theta; 5.57 psi 2024-11-06T04:35:19Z- handleDepth()- Depth_Keller msg: 1730867719.76 epoch sec; seqNo:7; depth 2.23 2024-11-06T04:35:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867719.79 epoch sec; seqNo:7; 4061474.88 north; 594482.99 east; 36.69 lat; -121.94 lon; 2.23 depth; utmZone: 10 2024-11-06T04:35:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867719.39 2024-11-06T04:35:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:19Z- Waiting for fresh data: AHRS(1730867719.76), Dvl(-1.00), Nav(1730867719.79), Depth(1730867719.76) 2024-11-06T04:35:20Z- AHRS_M2 msg: 1730867720.16 epoch sec; seqNo:7 2024-11-06T04:35:20Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.58 psi 2024-11-06T04:35:20Z- handleDepth()- Depth_Keller msg: 1730867720.17 epoch sec; seqNo:8; depth 2.27 2024-11-06T04:35:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867720.18 epoch sec; seqNo:8; 4061474.86 north; 594481.77 east; 36.69 lat; -121.94 lon; 2.27 depth; utmZone: 10 2024-11-06T04:35:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867719.39 2024-11-06T04:35:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:20Z- Waiting for fresh data: AHRS(1730867720.16), Dvl(-1.00), Nav(1730867720.18), Depth(1730867720.17) 2024-11-06T04:35:20Z- AHRS_M2 msg: 1730867720.57 epoch sec; seqNo:8 2024-11-06T04:35:20Z- AHRS_M2 msg: 0.13 phi; -0.15 theta; 5.58 psi 2024-11-06T04:35:20Z- handleDepth()- Depth_Keller msg: 1730867720.57 epoch sec; seqNo:9; depth 2.33 2024-11-06T04:35:20Z- handleDVL() - DVL msg epochMillisec: 1730867720.59 2024-11-06T04:35:20Z- handleDvl() - RDI_Pathfinder msg: 1730867720.59 sec; seqNo:2; #beams:4 2024-11-06T04:35:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.78 , 84.60 , 78.92 , 90.69 2024-11-06T04:35:20Z- handleDvl() - RDI_Pathfinder msg: velocities 1.14 , -0.13 , -0.47 2024-11-06T04:35:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867720.63 epoch sec; seqNo:9; 4061475.24 north; 594481.76 east; 36.69 lat; -121.94 lon; 2.33 depth; utmZone: 10 2024-11-06T04:35:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867719.39 2024-11-06T04:35:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:20Z- waiting for 1730867723.39, time is 1730867720.63 2024-11-06T04:35:20Z- AHRS_M2 msg: 1730867720.97 epoch sec; seqNo:9 2024-11-06T04:35:20Z- AHRS_M2 msg: 0.14 phi; -0.14 theta; 5.57 psi 2024-11-06T04:35:20Z- handleDepth()- Depth_Keller msg: 1730867720.98 epoch sec; seqNo:10; depth 2.37 2024-11-06T04:35:20Z- handleDVL() - DVL msg epochMillisec: 1730867720.98 2024-11-06T04:35:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:35:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:35:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:35:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:35:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:35:20Z- handleDvl() - RDI_Pathfinder msg: 1730867720.98 sec; seqNo:3; #beams:0 2024-11-06T04:35:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:35:20Z- handleDvl() - RDI_Pathfinder msg: velocities 1.14 , -0.13 , -0.47 2024-11-06T04:35:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867721.00 epoch sec; seqNo:10; 4061475.62 north; 594481.76 east; 36.69 lat; -121.94 lon; 2.37 depth; utmZone: 10 2024-11-06T04:35:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867719.39 2024-11-06T04:35:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:21Z- waiting for 1730867723.39, time is 1730867721.00 2024-11-06T04:35:21Z- AHRS_M2 msg: 1730867721.37 epoch sec; seqNo:10 2024-11-06T04:35:21Z- AHRS_M2 msg: 0.14 phi; -0.13 theta; 5.57 psi 2024-11-06T04:35:21Z- handleDepth()- Depth_Keller msg: 1730867721.38 epoch sec; seqNo:11; depth 2.42 2024-11-06T04:35:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867721.42 epoch sec; seqNo:11; 4061476.00 north; 594481.75 east; 36.69 lat; -121.94 lon; 2.42 depth; utmZone: 10 2024-11-06T04:35:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867719.39 2024-11-06T04:35:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:21Z- waiting for 1730867723.39, time is 1730867721.42 2024-11-06T04:35:21Z- AHRS_M2 msg: 1730867721.78 epoch sec; seqNo:11 2024-11-06T04:35:21Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.57 psi 2024-11-06T04:35:21Z- handleDepth()- Depth_Keller msg: 1730867721.79 epoch sec; seqNo:12; depth 2.47 2024-11-06T04:35:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867721.80 epoch sec; seqNo:12; 4061476.37 north; 594480.53 east; 36.69 lat; -121.94 lon; 2.47 depth; utmZone: 10 2024-11-06T04:35:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867719.39 2024-11-06T04:35:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:21Z- waiting for 1730867723.39, time is 1730867721.80 2024-11-06T04:35:22Z- AHRS_M2 msg: 1730867722.19 epoch sec; seqNo:12 2024-11-06T04:35:22Z- AHRS_M2 msg: 0.11 phi; -0.11 theta; 5.57 psi 2024-11-06T04:35:22Z- handleDepth()- Depth_Keller msg: 1730867722.20 epoch sec; seqNo:13; depth 2.53 2024-11-06T04:35:22Z- handleDVL() - DVL msg epochMillisec: 1730867722.20 2024-11-06T04:35:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:35:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:35:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:35:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:35:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:35:22Z- handleDvl() - RDI_Pathfinder msg: 1730867722.20 sec; seqNo:4; #beams:0 2024-11-06T04:35:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:35:22Z- handleDvl() - RDI_Pathfinder msg: velocities 1.14 , -0.13 , -0.47 2024-11-06T04:35:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867722.21 epoch sec; seqNo:13; 4061476.74 north; 594480.52 east; 36.69 lat; -121.94 lon; 2.53 depth; utmZone: 10 2024-11-06T04:35:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867719.39 2024-11-06T04:35:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:22Z- waiting for 1730867723.39, time is 1730867722.21 2024-11-06T04:35:22Z- AHRS_M2 msg: 1730867722.59 epoch sec; seqNo:13 2024-11-06T04:35:22Z- AHRS_M2 msg: 0.11 phi; -0.12 theta; 5.57 psi 2024-11-06T04:35:22Z- handleDepth()- Depth_Keller msg: 1730867722.59 epoch sec; seqNo:14; depth 2.58 2024-11-06T04:35:22Z- handleDVL() - DVL msg epochMillisec: 1730867722.62 2024-11-06T04:35:22Z- handleDvl() - RDI_Pathfinder msg: 1730867722.62 sec; seqNo:5; #beams:4 2024-11-06T04:35:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.78 , 78.51 , 83.39 , 88.26 2024-11-06T04:35:22Z- handleDvl() - RDI_Pathfinder msg: velocities 1.39 , -0.31 , 0.11 2024-11-06T04:35:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867722.65 epoch sec; seqNo:14; 4061477.12 north; 594480.52 east; 36.69 lat; -121.94 lon; 2.58 depth; utmZone: 10 2024-11-06T04:35:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867719.39 2024-11-06T04:35:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:22Z- waiting for 1730867723.39, time is 1730867722.65 2024-11-06T04:35:23Z- AHRS_M2 msg: 1730867723.00 epoch sec; seqNo:14 2024-11-06T04:35:23Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.56 psi 2024-11-06T04:35:23Z- handleDepth()- Depth_Keller msg: 1730867723.00 epoch sec; seqNo:15; depth 2.64 2024-11-06T04:35:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867723.03 epoch sec; seqNo:15; 4061477.49 north; 594479.30 east; 36.69 lat; -121.94 lon; 2.64 depth; utmZone: 10 2024-11-06T04:35:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867719.39 2024-11-06T04:35:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:23Z- waiting for 1730867723.39, time is 1730867723.03 2024-11-06T04:35:23Z- AHRS_M2 msg: 1730867723.40 epoch sec; seqNo:15 2024-11-06T04:35:23Z- AHRS_M2 msg: 0.12 phi; -0.14 theta; 5.55 psi 2024-11-06T04:35:23Z- handleDepth()- Depth_Keller msg: 1730867723.41 epoch sec; seqNo:16; depth 2.70 2024-11-06T04:35:23Z- handleDVL() - DVL msg epochMillisec: 1730867723.41 2024-11-06T04:35:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:35:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:35:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:35:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:35:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:35:23Z- handleDvl() - RDI_Pathfinder msg: 1730867723.41 sec; seqNo:6; #beams:0 2024-11-06T04:35:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:35:23Z- handleDvl() - RDI_Pathfinder msg: velocities 1.39 , -0.31 , 0.11 2024-11-06T04:35:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867723.42 epoch sec; seqNo:16; 4061477.49 north; 594479.30 east; 36.69 lat; -121.94 lon; 2.70 depth; utmZone: 10 2024-11-06T04:35:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867719.39 2024-11-06T04:35:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:23Z- waiting for 1730867723.39, time is 1730867723.42 2024-11-06T04:35:23Z- LcmTrn::updateTrn() >>>> pose time 1730867723.42, meas time 1730867723.41 <<<< 2024-11-06T04:35:23Z- LcmTrn::updateTrn() equating pose 1730867723.41 and meas times 1730867723.41 2024-11-06T04:35:23Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867723.41 2024-11-06T04:35:23Z- TerrainNav::motionUpdate() - currEstimate = (4061477, 594479.30, 2.70) 2024-11-06T04:35:23Z- TerrainNav::Filter not initialized - vehicle currently does not have good range measurements 2024-11-06T04:35:23Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:35:23Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2024-11-06T04:35:23Z- TerrainNav::Cannot compute pose estimate; motion has not been initialized. 2024-11-06T04:35:23Z- publishEstimatesDecoder() - at 1730867723.41 sec 2024-11-06T04:35:23Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:35:23Z- LcmTrn::publishEstimatesDecoder() - MLE : 0.00 0.00 0.00 2024-11-06T04:35:23Z- LcmTrn::publishEstimatesDecoder() - MMSE : 0.00 0.00 0.00 lat/lon: 0.00 -127.49 2024-11-06T04:35:23Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.00 0.00 0.00 0.00 2024-11-06T04:35:23Z- AHRS_M2 msg: 1730867723.80 epoch sec; seqNo:16 2024-11-06T04:35:23Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.54 psi 2024-11-06T04:35:23Z- handleDepth()- Depth_Keller msg: 1730867723.82 epoch sec; seqNo:17; depth 2.74 2024-11-06T04:35:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867723.83 epoch sec; seqNo:17; 4061477.87 north; 594479.29 east; 36.69 lat; -121.94 lon; 2.74 depth; utmZone: 10 2024-11-06T04:35:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867723.41 2024-11-06T04:35:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:23Z- waiting for 1730867727.41, time is 1730867723.83 2024-11-06T04:35:24Z- AHRS_M2 msg: 1730867724.20 epoch sec; seqNo:17 2024-11-06T04:35:24Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.54 psi 2024-11-06T04:35:24Z- handleDepth()- Depth_Keller msg: 1730867724.21 epoch sec; seqNo:18; depth 2.80 2024-11-06T04:35:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867724.23 epoch sec; seqNo:18; 4061478.23 north; 594478.07 east; 36.69 lat; -121.94 lon; 2.80 depth; utmZone: 10 2024-11-06T04:35:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867723.41 2024-11-06T04:35:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:24Z- waiting for 1730867727.41, time is 1730867724.23 2024-11-06T04:35:24Z- AHRS_M2 msg: 1730867724.61 epoch sec; seqNo:18 2024-11-06T04:35:24Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.53 psi 2024-11-06T04:35:24Z- handleDepth()- Depth_Keller msg: 1730867724.62 epoch sec; seqNo:19; depth 2.86 2024-11-06T04:35:24Z- handleDVL() - DVL msg epochMillisec: 1730867724.66 2024-11-06T04:35:24Z- handleDvl() - RDI_Pathfinder msg: 1730867724.66 sec; seqNo:7; #beams:4 2024-11-06T04:35:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.85 , 78.92 , 83.45 , 90.76 2024-11-06T04:35:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , -0.18 , 0.29 2024-11-06T04:35:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867724.70 epoch sec; seqNo:19; 4061478.61 north; 594478.06 east; 36.69 lat; -121.94 lon; 2.86 depth; utmZone: 10 2024-11-06T04:35:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867723.41 2024-11-06T04:35:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:24Z- waiting for 1730867727.41, time is 1730867724.70 2024-11-06T04:35:25Z- handleDepth()- Depth_Keller msg: 1730867725.06 epoch sec; seqNo:20; depth 2.89 2024-11-06T04:35:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867725.08 epoch sec; seqNo:20; 4061478.61 north; 594478.06 east; 36.69 lat; -121.94 lon; 2.89 depth; utmZone: 10 2024-11-06T04:35:25Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:35:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867723.41 2024-11-06T04:35:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:25Z- waiting for 1730867727.41, time is 1730867725.08 2024-11-06T04:35:25Z- AHRS_M2 msg: 1730867725.41 epoch sec; seqNo:19 2024-11-06T04:35:25Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.53 psi 2024-11-06T04:35:25Z- handleDepth()- Depth_Keller msg: 1730867725.43 epoch sec; seqNo:21; depth 2.94 2024-11-06T04:35:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867725.44 epoch sec; seqNo:21; 4061478.98 north; 594476.84 east; 36.69 lat; -121.94 lon; 2.94 depth; utmZone: 10 2024-11-06T04:35:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867723.41 2024-11-06T04:35:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:25Z- waiting for 1730867727.41, time is 1730867725.44 2024-11-06T04:35:25Z- AHRS_M2 msg: 1730867725.82 epoch sec; seqNo:20 2024-11-06T04:35:25Z- AHRS_M2 msg: 0.11 phi; -0.16 theta; 5.53 psi 2024-11-06T04:35:25Z- handleDepth()- Depth_Keller msg: 1730867725.83 epoch sec; seqNo:22; depth 2.97 2024-11-06T04:35:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867725.86 epoch sec; seqNo:22; 4061479.36 north; 594476.83 east; 36.69 lat; -121.94 lon; 2.97 depth; utmZone: 10 2024-11-06T04:35:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867723.41 2024-11-06T04:35:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:25Z- waiting for 1730867727.41, time is 1730867725.86 2024-11-06T04:35:26Z- AHRS_M2 msg: 1730867726.22 epoch sec; seqNo:21 2024-11-06T04:35:26Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.53 psi 2024-11-06T04:35:26Z- handleDepth()- Depth_Keller msg: 1730867726.23 epoch sec; seqNo:23; depth 3.02 2024-11-06T04:35:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867726.24 epoch sec; seqNo:23; 4061479.36 north; 594476.83 east; 36.69 lat; -121.94 lon; 3.02 depth; utmZone: 10 2024-11-06T04:35:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867723.41 2024-11-06T04:35:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:26Z- waiting for 1730867727.41, time is 1730867726.24 2024-11-06T04:35:26Z- AHRS_M2 msg: 1730867726.63 epoch sec; seqNo:22 2024-11-06T04:35:26Z- AHRS_M2 msg: 0.12 phi; -0.17 theta; 5.53 psi 2024-11-06T04:35:26Z- handleDepth()- Depth_Keller msg: 1730867726.64 epoch sec; seqNo:24; depth 3.05 2024-11-06T04:35:26Z- handleDVL() - DVL msg epochMillisec: 1730867726.66 2024-11-06T04:35:26Z- handleDvl() - RDI_Pathfinder msg: 1730867726.66 sec; seqNo:8; #beams:4 2024-11-06T04:35:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.78 , 78.92 , 83.39 , 90.69 2024-11-06T04:35:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.67 , -0.13 , 0.05 2024-11-06T04:35:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867726.67 epoch sec; seqNo:24; 4061479.73 north; 594476.83 east; 36.69 lat; -121.94 lon; 3.05 depth; utmZone: 10 2024-11-06T04:35:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867723.41 2024-11-06T04:35:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:26Z- waiting for 1730867727.41, time is 1730867726.67 2024-11-06T04:35:27Z- AHRS_M2 msg: 1730867727.03 epoch sec; seqNo:23 2024-11-06T04:35:27Z- AHRS_M2 msg: 0.12 phi; -0.17 theta; 5.53 psi 2024-11-06T04:35:27Z- handleDepth()- Depth_Keller msg: 1730867727.04 epoch sec; seqNo:25; depth 3.08 2024-11-06T04:35:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867727.06 epoch sec; seqNo:25; 4061479.72 north; 594475.61 east; 36.69 lat; -121.94 lon; 3.08 depth; utmZone: 10 2024-11-06T04:35:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867723.41 2024-11-06T04:35:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:27Z- waiting for 1730867727.41, time is 1730867727.06 2024-11-06T04:35:27Z- AHRS_M2 msg: 1730867727.44 epoch sec; seqNo:24 2024-11-06T04:35:27Z- AHRS_M2 msg: 0.12 phi; -0.16 theta; 5.53 psi 2024-11-06T04:35:27Z- handleDepth()- Depth_Keller msg: 1730867727.45 epoch sec; seqNo:26; depth 3.11 2024-11-06T04:35:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867727.46 epoch sec; seqNo:26; 4061479.72 north; 594475.61 east; 36.69 lat; -121.94 lon; 3.11 depth; utmZone: 10 2024-11-06T04:35:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867723.41 2024-11-06T04:35:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:27Z- waiting for 1730867727.41, time is 1730867727.46 2024-11-06T04:35:27Z- LcmTrn::updateTrn() >>>> pose time 1730867727.46, meas time 1730867726.66 <<<< 2024-11-06T04:35:27Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867727.46 2024-11-06T04:35:27Z- TerrainNav::Measurement type 1 from time = 1730867726.66 sec. not included; vehicle motion has not been initialized. lastMeasValid = 1, beams = 4, ping # 0 2024-11-06T04:35:27Z- TerrainNav::motionUpdate() - currEstimate = (4061480, 594475.61, 3.11) 2024-11-06T04:35:27Z- TerrainNav::attemptInitFilter is increasing Init Search Window by 0.000000 m 2024-11-06T04:35:27Z- TNAVPF: Successfully initialized particles 2024-11-06T04:35:27Z- TerrainNav:: vehicle motion has been initialized 2024-11-06T04:35:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:35:27Z- variances([0], [2], and psi_berg) = 1206.462059 1205.046219 0.000000 2024-11-06T04:35:27Z- publishEstimatesDecoder() - at 1730867727.46 sec 2024-11-06T04:35:27Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:35:27Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061523.90 594489.77 5.37 2024-11-06T04:35:27Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061479.67 594475.87 3.11 lat/lon: 36.69 -121.94 2024-11-06T04:35:27Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1206.46 1205.05 8.43 0.00 2024-11-06T04:35:27Z- handleDepth()- Depth_Keller msg: 1730867727.87 epoch sec; seqNo:27; depth 3.15 2024-11-06T04:35:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867727.88 epoch sec; seqNo:27; 4061480.10 north; 594475.61 east; 36.69 lat; -121.94 lon; 3.15 depth; utmZone: 10 2024-11-06T04:35:28Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:35:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867727.46 2024-11-06T04:35:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:28Z- waiting for 1730867731.46, time is 1730867727.88 2024-11-06T04:35:28Z- AHRS_M2 msg: 1730867728.24 epoch sec; seqNo:25 2024-11-06T04:35:28Z- AHRS_M2 msg: 0.10 phi; -0.13 theta; 5.53 psi 2024-11-06T04:35:28Z- handleDepth()- Depth_Keller msg: 1730867728.25 epoch sec; seqNo:28; depth 3.18 2024-11-06T04:35:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867728.27 epoch sec; seqNo:28; 4061480.10 north; 594475.61 east; 36.69 lat; -121.94 lon; 3.18 depth; utmZone: 10 2024-11-06T04:35:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867727.46 2024-11-06T04:35:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:28Z- waiting for 1730867731.46, time is 1730867728.27 2024-11-06T04:35:28Z- AHRS_M2 msg: 1730867728.65 epoch sec; seqNo:26 2024-11-06T04:35:28Z- AHRS_M2 msg: 0.10 phi; -0.12 theta; 5.53 psi 2024-11-06T04:35:28Z- handleDepth()- Depth_Keller msg: 1730867728.65 epoch sec; seqNo:29; depth 3.25 2024-11-06T04:35:28Z- handleDVL() - DVL msg epochMillisec: 1730867728.68 2024-11-06T04:35:28Z- handleDvl() - RDI_Pathfinder msg: 1730867728.68 sec; seqNo:9; #beams:4 2024-11-06T04:35:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.56 , 78.92 , 78.92 , 90.69 2024-11-06T04:35:28Z- handleDvl() - RDI_Pathfinder msg: velocities 1.14 , -0.14 , -0.05 2024-11-06T04:35:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867728.70 epoch sec; seqNo:29; 4061480.48 north; 594475.60 east; 36.69 lat; -121.94 lon; 3.25 depth; utmZone: 10 2024-11-06T04:35:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867727.46 2024-11-06T04:35:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:28Z- waiting for 1730867731.46, time is 1730867728.70 2024-11-06T04:35:29Z- AHRS_M2 msg: 1730867729.05 epoch sec; seqNo:27 2024-11-06T04:35:29Z- AHRS_M2 msg: 0.11 phi; -0.12 theta; 5.53 psi 2024-11-06T04:35:29Z- handleDepth()- Depth_Keller msg: 1730867729.06 epoch sec; seqNo:30; depth 3.30 2024-11-06T04:35:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867729.08 epoch sec; seqNo:30; 4061480.84 north; 594474.38 east; 36.69 lat; -121.94 lon; 3.30 depth; utmZone: 10 2024-11-06T04:35:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867727.46 2024-11-06T04:35:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:29Z- waiting for 1730867731.46, time is 1730867729.08 2024-11-06T04:35:29Z- AHRS_M2 msg: 1730867729.45 epoch sec; seqNo:28 2024-11-06T04:35:29Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.53 psi 2024-11-06T04:35:29Z- handleDepth()- Depth_Keller msg: 1730867729.46 epoch sec; seqNo:31; depth 3.36 2024-11-06T04:35:29Z- handleDVL() - DVL msg epochMillisec: 1730867729.47 2024-11-06T04:35:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:35:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:35:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:35:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:35:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:35:29Z- handleDvl() - RDI_Pathfinder msg: 1730867729.47 sec; seqNo:10; #beams:0 2024-11-06T04:35:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:35:29Z- handleDvl() - RDI_Pathfinder msg: velocities 1.14 , -0.14 , -0.05 2024-11-06T04:35:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867729.48 epoch sec; seqNo:31; 4061481.22 north; 594474.37 east; 36.69 lat; -121.94 lon; 3.36 depth; utmZone: 10 2024-11-06T04:35:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867727.46 2024-11-06T04:35:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:29Z- waiting for 1730867731.46, time is 1730867729.48 2024-11-06T04:35:29Z- AHRS_M2 msg: 1730867729.86 epoch sec; seqNo:29 2024-11-06T04:35:29Z- AHRS_M2 msg: 0.14 phi; -0.14 theta; 5.53 psi 2024-11-06T04:35:29Z- handleDepth()- Depth_Keller msg: 1730867729.87 epoch sec; seqNo:32; depth 3.43 2024-11-06T04:35:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867729.88 epoch sec; seqNo:32; 4061481.22 north; 594474.37 east; 36.69 lat; -121.94 lon; 3.43 depth; utmZone: 10 2024-11-06T04:35:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867727.46 2024-11-06T04:35:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:30Z- waiting for 1730867731.46, time is 1730867729.88 2024-11-06T04:35:30Z- handleDepth()- Depth_Keller msg: 1730867730.29 epoch sec; seqNo:33; depth 3.48 2024-11-06T04:35:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867730.30 epoch sec; seqNo:33; 4061481.59 north; 594473.15 east; 36.69 lat; -121.94 lon; 3.48 depth; utmZone: 10 2024-11-06T04:35:30Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:35:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867727.46 2024-11-06T04:35:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:30Z- waiting for 1730867731.46, time is 1730867730.30 2024-11-06T04:35:30Z- AHRS_M2 msg: 1730867730.67 epoch sec; seqNo:30 2024-11-06T04:35:30Z- AHRS_M2 msg: 0.16 phi; -0.17 theta; 5.54 psi 2024-11-06T04:35:30Z- handleDepth()- Depth_Keller msg: 1730867730.68 epoch sec; seqNo:34; depth 3.54 2024-11-06T04:35:30Z- handleDVL() - DVL msg epochMillisec: 1730867730.70 2024-11-06T04:35:30Z- handleDvl() - RDI_Pathfinder msg: 1730867730.70 sec; seqNo:11; #beams:4 2024-11-06T04:35:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.92 , 78.92 , 84.74 , 90.83 2024-11-06T04:35:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , -0.07 , 0.34 2024-11-06T04:35:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867730.71 epoch sec; seqNo:34; 4061481.97 north; 594473.14 east; 36.69 lat; -121.94 lon; 3.54 depth; utmZone: 10 2024-11-06T04:35:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867727.46 2024-11-06T04:35:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:30Z- waiting for 1730867731.46, time is 1730867730.71 2024-11-06T04:35:31Z- AHRS_M2 msg: 1730867731.07 epoch sec; seqNo:31 2024-11-06T04:35:31Z- AHRS_M2 msg: 0.16 phi; -0.18 theta; 5.54 psi 2024-11-06T04:35:31Z- handleDepth()- Depth_Keller msg: 1730867731.08 epoch sec; seqNo:35; depth 3.58 2024-11-06T04:35:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867731.10 epoch sec; seqNo:35; 4061481.97 north; 594473.14 east; 36.69 lat; -121.94 lon; 3.58 depth; utmZone: 10 2024-11-06T04:35:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867727.46 2024-11-06T04:35:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:31Z- waiting for 1730867731.46, time is 1730867731.10 2024-11-06T04:35:31Z- AHRS_M2 msg: 1730867731.47 epoch sec; seqNo:32 2024-11-06T04:35:31Z- AHRS_M2 msg: 0.15 phi; -0.19 theta; 5.54 psi 2024-11-06T04:35:31Z- handleDepth()- Depth_Keller msg: 1730867731.49 epoch sec; seqNo:36; depth 3.62 2024-11-06T04:35:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867731.51 epoch sec; seqNo:36; 4061481.97 north; 594473.14 east; 36.69 lat; -121.94 lon; 3.62 depth; utmZone: 10 2024-11-06T04:35:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867727.46 2024-11-06T04:35:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:31Z- waiting for 1730867731.46, time is 1730867731.51 2024-11-06T04:35:31Z- LcmTrn::updateTrn() >>>> pose time 1730867731.51, meas time 1730867730.70 <<<< 2024-11-06T04:35:31Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867731.51 2024-11-06T04:35:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867730.70 sec, ping # 0; waiting for INS data more recent than 1730867727.46... 2024-11-06T04:35:31Z- TerrainNav::motionUpdate() - currEstimate = (4061482, 594473.14, 3.62) 2024-11-06T04:35:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:35:31Z- TerrainNav:: SubcloudNIS is 0.00, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:35:31Z- TNavPF::Projecting Measurements 2024-11-06T04:35:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:35:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:35:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:35:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:35:31Z- TNavFilter::4 good beams. 2024-11-06T04:35:31Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:35:31Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:35:31Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:35:31Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:35:31Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:35:31Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:35:31Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:35:31Z- TerrainMapOctree: (vehN, vehE ) = (4061481.52, 594473.64). 2024-11-06T04:35:31Z- TerrainMapOctree: Min Distance = 321.89. 2024-11-06T04:35:31Z- TerrainMapOctree: Using tile 1. 2024-11-06T04:35:31Z- Forcing Subcloud Comparison 2024-11-06T04:35:31Z- Weighting particles with subcloud comparison 2024-11-06T04:35:31Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:31Z- meanExpectedMeasDiff: -14.712491 delta_rms_squared: 21.759025 alpha: 0.999082 2024-11-06T04:35:31Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:31Z- meanExpectedMeasDiff: -11.295000 delta_rms_squared: 14.208822 alpha: 0.998594 2024-11-06T04:35:31Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:31Z- meanExpectedMeasDiff: -6.062821 delta_rms_squared: 13.723271 alpha: 0.998545 2024-11-06T04:35:31Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:31Z- meanExpectedMeasDiff: -18.986005 delta_rms_squared: 22.676126 alpha: 0.999119 2024-11-06T04:35:31Z- Subcloud NIS: 0.126870 2024-11-06T04:35:31Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:35:31Z- TNavPF:: sumWeights = 0.000138 2024-11-06T04:35:31Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:35:31Z- TNavPF::First Term of Map Covariance Matrix is 0.01 2024-11-06T04:35:31Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:35:31Z- TerrainNav::Resampling particle filter... 2024-11-06T04:35:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867730.70 sec. 2024-11-06T04:35:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:35:31Z- variances([0], [2], and psi_berg) = 933.690956 413.309849 0.000000 2024-11-06T04:35:31Z- publishEstimatesDecoder() - at 1730867731.51 sec 2024-11-06T04:35:31Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:35:31Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061423.21 594493.60 7.85 2024-11-06T04:35:31Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061479.16 594503.82 8.56 lat/lon: 36.69 -121.94 2024-11-06T04:35:31Z- LcmTrn::publishEstimatesDecoder() - COVAR: 933.69 413.31 0.63 0.00 2024-11-06T04:35:31Z- AHRS_M2 msg: 1730867731.89 epoch sec; seqNo:33 2024-11-06T04:35:31Z- AHRS_M2 msg: 0.15 phi; -0.20 theta; 5.55 psi 2024-11-06T04:35:31Z- handleDepth()- Depth_Keller msg: 1730867731.90 epoch sec; seqNo:37; depth 3.65 2024-11-06T04:35:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867731.92 epoch sec; seqNo:37; 4061482.35 north; 594473.14 east; 36.69 lat; -121.94 lon; 3.65 depth; utmZone: 10 2024-11-06T04:35:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867731.51 2024-11-06T04:35:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:32Z- waiting for 1730867735.51, time is 1730867731.92 2024-11-06T04:35:32Z- AHRS_M2 msg: 1730867732.28 epoch sec; seqNo:34 2024-11-06T04:35:32Z- AHRS_M2 msg: 0.15 phi; -0.22 theta; 5.55 psi 2024-11-06T04:35:32Z- handleDepth()- Depth_Keller msg: 1730867732.30 epoch sec; seqNo:38; depth 3.70 2024-11-06T04:35:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867732.31 epoch sec; seqNo:38; 4061482.35 north; 594473.14 east; 36.69 lat; -121.94 lon; 3.70 depth; utmZone: 10 2024-11-06T04:35:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867731.51 2024-11-06T04:35:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:32Z- waiting for 1730867735.51, time is 1730867732.31 2024-11-06T04:35:32Z- AHRS_M2 msg: 1730867732.69 epoch sec; seqNo:35 2024-11-06T04:35:32Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.54 psi 2024-11-06T04:35:32Z- handleDepth()- Depth_Keller msg: 1730867732.69 epoch sec; seqNo:39; depth 3.74 2024-11-06T04:35:32Z- handleDVL() - DVL msg epochMillisec: 1730867732.71 2024-11-06T04:35:32Z- handleDvl() - RDI_Pathfinder msg: 1730867732.71 sec; seqNo:12; #beams:4 2024-11-06T04:35:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.78 , 78.51 , 79.32 , 93.13 2024-11-06T04:35:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.53 , 0.08 , -0.29 2024-11-06T04:35:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867732.74 epoch sec; seqNo:39; 4061482.72 north; 594473.14 east; 36.69 lat; -121.94 lon; 3.74 depth; utmZone: 10 2024-11-06T04:35:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867731.51 2024-11-06T04:35:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:32Z- waiting for 1730867735.51, time is 1730867732.74 2024-11-06T04:35:33Z- AHRS_M2 msg: 1730867733.09 epoch sec; seqNo:36 2024-11-06T04:35:33Z- AHRS_M2 msg: 0.14 phi; -0.24 theta; 5.54 psi 2024-11-06T04:35:33Z- handleDepth()- Depth_Keller msg: 1730867733.10 epoch sec; seqNo:40; depth 3.78 2024-11-06T04:35:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867733.11 epoch sec; seqNo:40; 4061482.71 north; 594471.91 east; 36.69 lat; -121.94 lon; 3.78 depth; utmZone: 10 2024-11-06T04:35:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867731.51 2024-11-06T04:35:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:33Z- waiting for 1730867735.51, time is 1730867733.11 2024-11-06T04:35:33Z- AHRS_M2 msg: 1730867733.50 epoch sec; seqNo:37 2024-11-06T04:35:33Z- AHRS_M2 msg: 0.12 phi; -0.23 theta; 5.53 psi 2024-11-06T04:35:33Z- handleDepth()- Depth_Keller msg: 1730867733.51 epoch sec; seqNo:41; depth 3.83 2024-11-06T04:35:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867733.52 epoch sec; seqNo:41; 4061483.09 north; 594471.91 east; 36.69 lat; -121.94 lon; 3.83 depth; utmZone: 10 2024-11-06T04:35:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867731.51 2024-11-06T04:35:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:33Z- waiting for 1730867735.51, time is 1730867733.52 2024-11-06T04:35:33Z- AHRS_M2 msg: 1730867733.90 epoch sec; seqNo:38 2024-11-06T04:35:33Z- AHRS_M2 msg: 0.09 phi; -0.20 theta; 5.53 psi 2024-11-06T04:35:33Z- handleDepth()- Depth_Keller msg: 1730867733.90 epoch sec; seqNo:42; depth 3.85 2024-11-06T04:35:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867733.91 epoch sec; seqNo:42; 4061483.09 north; 594471.91 east; 36.69 lat; -121.94 lon; 3.85 depth; utmZone: 10 2024-11-06T04:35:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867731.51 2024-11-06T04:35:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:34Z- waiting for 1730867735.51, time is 1730867733.91 2024-11-06T04:35:34Z- AHRS_M2 msg: 1730867734.30 epoch sec; seqNo:39 2024-11-06T04:35:34Z- AHRS_M2 msg: 0.07 phi; -0.18 theta; 5.53 psi 2024-11-06T04:35:34Z- handleDepth()- Depth_Keller msg: 1730867734.31 epoch sec; seqNo:43; depth 3.90 2024-11-06T04:35:34Z- handleDVL() - DVL msg epochMillisec: 1730867734.32 2024-11-06T04:35:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:35:34Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:35:34Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:35:34Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:35:34Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:35:34Z- handleDvl() - RDI_Pathfinder msg: 1730867734.32 sec; seqNo:13; #beams:0 2024-11-06T04:35:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:35:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.53 , 0.08 , -0.29 2024-11-06T04:35:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867734.33 epoch sec; seqNo:43; 4061483.47 north; 594471.91 east; 36.69 lat; -121.94 lon; 3.90 depth; utmZone: 10 2024-11-06T04:35:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867731.51 2024-11-06T04:35:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:34Z- waiting for 1730867735.51, time is 1730867734.33 2024-11-06T04:35:34Z- AHRS_M2 msg: 1730867734.71 epoch sec; seqNo:40 2024-11-06T04:35:34Z- AHRS_M2 msg: 0.06 phi; -0.17 theta; 5.53 psi 2024-11-06T04:35:34Z- handleDepth()- Depth_Keller msg: 1730867734.71 epoch sec; seqNo:44; depth 3.97 2024-11-06T04:35:34Z- handleDVL() - DVL msg epochMillisec: 1730867734.73 2024-11-06T04:35:34Z- handleDvl() - RDI_Pathfinder msg: 1730867734.73 sec; seqNo:14; #beams:4 2024-11-06T04:35:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.41 , 79.32 , 76.05 , 94.41 2024-11-06T04:35:34Z- handleDvl() - RDI_Pathfinder msg: velocities 1.45 , -0.17 , -0.50 2024-11-06T04:35:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867734.76 epoch sec; seqNo:44; 4061483.85 north; 594471.90 east; 36.69 lat; -121.94 lon; 3.97 depth; utmZone: 10 2024-11-06T04:35:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867731.51 2024-11-06T04:35:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:34Z- waiting for 1730867735.51, time is 1730867734.76 2024-11-06T04:35:35Z- AHRS_M2 msg: 1730867735.11 epoch sec; seqNo:41 2024-11-06T04:35:35Z- AHRS_M2 msg: 0.07 phi; -0.17 theta; 5.53 psi 2024-11-06T04:35:35Z- handleDepth()- Depth_Keller msg: 1730867735.12 epoch sec; seqNo:45; depth 4.06 2024-11-06T04:35:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867735.14 epoch sec; seqNo:45; 4061484.21 north; 594470.68 east; 36.69 lat; -121.94 lon; 4.06 depth; utmZone: 10 2024-11-06T04:35:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867731.51 2024-11-06T04:35:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:35Z- waiting for 1730867735.51, time is 1730867735.14 2024-11-06T04:35:35Z- AHRS_M2 msg: 1730867735.52 epoch sec; seqNo:42 2024-11-06T04:35:35Z- AHRS_M2 msg: 0.09 phi; -0.17 theta; 5.53 psi 2024-11-06T04:35:35Z- handleDepth()- Depth_Keller msg: 1730867735.54 epoch sec; seqNo:46; depth 4.15 2024-11-06T04:35:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867735.55 epoch sec; seqNo:46; 4061484.59 north; 594470.67 east; 36.69 lat; -121.94 lon; 4.15 depth; utmZone: 10 2024-11-06T04:35:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867731.51 2024-11-06T04:35:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:35Z- waiting for 1730867735.51, time is 1730867735.55 2024-11-06T04:35:35Z- LcmTrn::updateTrn() >>>> pose time 1730867735.55, meas time 1730867734.73 <<<< 2024-11-06T04:35:35Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867735.55 2024-11-06T04:35:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867734.73 sec, ping # 0; waiting for INS data more recent than 1730867731.51... 2024-11-06T04:35:35Z- TerrainNav::motionUpdate() - currEstimate = (4061485, 594470.67, 4.15) 2024-11-06T04:35:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:35:35Z- TerrainNav:: SubcloudNIS is 0.13, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:35:35Z- TNavPF::Projecting Measurements 2024-11-06T04:35:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:35:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:35:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:35:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:35:35Z- TNavFilter::4 good beams. 2024-11-06T04:35:35Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:35:35Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:35:35Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:35:35Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:35:35Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:35:35Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:35:35Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:35:35Z- TerrainMapOctree: (vehN, vehE ) = (4061484.06, 594471.17). 2024-11-06T04:35:35Z- TerrainMapOctree: Min Distance = 322.03. 2024-11-06T04:35:35Z- TerrainMapOctree: Using tile 1. 2024-11-06T04:35:35Z- Forcing Subcloud Comparison 2024-11-06T04:35:35Z- Weighting particles with subcloud comparison 2024-11-06T04:35:35Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:35Z- meanExpectedMeasDiff: -3.837296 delta_rms_squared: 5.282836 alpha: 0.996228 2024-11-06T04:35:35Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:35Z- meanExpectedMeasDiff: -4.920415 delta_rms_squared: 4.561070 alpha: 0.995634 2024-11-06T04:35:35Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:35Z- meanExpectedMeasDiff: -8.221304 delta_rms_squared: 1.987197 alpha: 0.990036 2024-11-06T04:35:35Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:35Z- meanExpectedMeasDiff: -3.818430 delta_rms_squared: 6.182868 alpha: 0.996776 2024-11-06T04:35:35Z- Subcloud NIS: 0.083193 2024-11-06T04:35:35Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:35:35Z- TNavPF:: sumWeights = 0.018339 2024-11-06T04:35:35Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:35:35Z- TNavPF::First Term of Map Covariance Matrix is 0.10 2024-11-06T04:35:35Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:35:35Z- TerrainNav::Resampling particle filter... 2024-11-06T04:35:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867734.73 sec. 2024-11-06T04:35:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:35:35Z- variances([0], [2], and psi_berg) = 788.673215 187.612129 0.000000 2024-11-06T04:35:35Z- publishEstimatesDecoder() - at 1730867735.55 sec 2024-11-06T04:35:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:35:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061425.86 594491.11 9.85 2024-11-06T04:35:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061501.80 594514.64 10.07 lat/lon: 36.69 -121.94 2024-11-06T04:35:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 788.67 187.61 0.82 0.00 2024-11-06T04:35:35Z- AHRS_M2 msg: 1730867735.92 epoch sec; seqNo:43 2024-11-06T04:35:35Z- AHRS_M2 msg: 0.10 phi; -0.18 theta; 5.52 psi 2024-11-06T04:35:35Z- handleDepth()- Depth_Keller msg: 1730867735.95 epoch sec; seqNo:47; depth 4.22 2024-11-06T04:35:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867735.97 epoch sec; seqNo:47; 4061484.97 north; 594470.67 east; 36.69 lat; -121.94 lon; 4.22 depth; utmZone: 10 2024-11-06T04:35:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867735.55 2024-11-06T04:35:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:36Z- waiting for 1730867739.55, time is 1730867735.97 2024-11-06T04:35:36Z- AHRS_M2 msg: 1730867736.33 epoch sec; seqNo:44 2024-11-06T04:35:36Z- AHRS_M2 msg: 0.10 phi; -0.18 theta; 5.51 psi 2024-11-06T04:35:36Z- handleDepth()- Depth_Keller msg: 1730867736.33 epoch sec; seqNo:48; depth 4.29 2024-11-06T04:35:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867736.38 epoch sec; seqNo:48; 4061485.34 north; 594469.44 east; 36.69 lat; -121.94 lon; 4.29 depth; utmZone: 10 2024-11-06T04:35:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867735.55 2024-11-06T04:35:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:36Z- waiting for 1730867739.55, time is 1730867736.38 2024-11-06T04:35:36Z- handleDepth()- Depth_Keller msg: 1730867736.75 epoch sec; seqNo:49; depth 4.37 2024-11-06T04:35:36Z- handleDVL() - DVL msg epochMillisec: 1730867736.77 2024-11-06T04:35:36Z- handleDvl() - RDI_Pathfinder msg: 1730867736.77 sec; seqNo:15; #beams:4 2024-11-06T04:35:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.04 , 78.51 , 77.28 , 89.61 2024-11-06T04:35:36Z- handleDvl() - RDI_Pathfinder msg: velocities 1.12 , -0.27 , 0.19 2024-11-06T04:35:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867736.79 epoch sec; seqNo:49; 4061485.72 north; 594469.44 east; 36.69 lat; -121.94 lon; 4.37 depth; utmZone: 10 2024-11-06T04:35:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867735.55 2024-11-06T04:35:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:36Z- waiting for 1730867739.55, time is 1730867736.79 2024-11-06T04:35:37Z- AHRS_M2 msg: 1730867737.13 epoch sec; seqNo:45 2024-11-06T04:35:37Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.52 psi 2024-11-06T04:35:37Z- handleDepth()- Depth_Keller msg: 1730867737.14 epoch sec; seqNo:50; depth 4.41 2024-11-06T04:35:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867737.15 epoch sec; seqNo:50; 4061485.72 north; 594469.44 east; 36.69 lat; -121.94 lon; 4.41 depth; utmZone: 10 2024-11-06T04:35:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867735.55 2024-11-06T04:35:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:37Z- waiting for 1730867739.55, time is 1730867737.15 2024-11-06T04:35:37Z- AHRS_M2 msg: 1730867737.54 epoch sec; seqNo:46 2024-11-06T04:35:37Z- AHRS_M2 msg: 0.12 phi; -0.14 theta; 5.53 psi 2024-11-06T04:35:37Z- handleDepth()- Depth_Keller msg: 1730867737.55 epoch sec; seqNo:51; depth 4.47 2024-11-06T04:35:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867737.56 epoch sec; seqNo:51; 4061486.08 north; 594468.22 east; 36.69 lat; -121.94 lon; 4.47 depth; utmZone: 10 2024-11-06T04:35:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867735.55 2024-11-06T04:35:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:37Z- waiting for 1730867739.55, time is 1730867737.56 2024-11-06T04:35:37Z- AHRS_M2 msg: 1730867737.94 epoch sec; seqNo:47 2024-11-06T04:35:37Z- AHRS_M2 msg: 0.13 phi; -0.12 theta; 5.52 psi 2024-11-06T04:35:37Z- handleDepth()- Depth_Keller msg: 1730867737.94 epoch sec; seqNo:52; depth 4.53 2024-11-06T04:35:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867737.97 epoch sec; seqNo:52; 4061486.46 north; 594468.21 east; 36.69 lat; -121.94 lon; 4.53 depth; utmZone: 10 2024-11-06T04:35:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867735.55 2024-11-06T04:35:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:38Z- waiting for 1730867739.55, time is 1730867737.97 2024-11-06T04:35:38Z- AHRS_M2 msg: 1730867738.38 epoch sec; seqNo:48 2024-11-06T04:35:38Z- AHRS_M2 msg: 0.12 phi; -0.09 theta; 5.52 psi 2024-11-06T04:35:38Z- handleDepth()- Depth_Keller msg: 1730867738.41 epoch sec; seqNo:53; depth 4.58 2024-11-06T04:35:38Z- handleDVL() - DVL msg epochMillisec: 1730867738.44 2024-11-06T04:35:38Z- handleDvl() - RDI_Pathfinder msg: 1730867738.44 sec; seqNo:16; #beams:4 2024-11-06T04:35:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.83 , 76.87 , 79.32 , 89.74 2024-11-06T04:35:38Z- handleDvl() - RDI_Pathfinder msg: velocities 1.25 , -0.14 , 0.15 2024-11-06T04:35:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867738.47 epoch sec; seqNo:53; 4061486.84 north; 594468.21 east; 36.69 lat; -121.94 lon; 4.58 depth; utmZone: 10 2024-11-06T04:35:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867735.55 2024-11-06T04:35:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:38Z- waiting for 1730867739.55, time is 1730867738.47 2024-11-06T04:35:38Z- AHRS_M2 msg: 1730867738.75 epoch sec; seqNo:49 2024-11-06T04:35:38Z- AHRS_M2 msg: 0.11 phi; -0.07 theta; 5.52 psi 2024-11-06T04:35:38Z- handleDepth()- Depth_Keller msg: 1730867738.75 epoch sec; seqNo:54; depth 4.66 2024-11-06T04:35:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867738.79 epoch sec; seqNo:54; 4061486.82 north; 594466.99 east; 36.69 lat; -121.94 lon; 4.66 depth; utmZone: 10 2024-11-06T04:35:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867735.55 2024-11-06T04:35:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:38Z- waiting for 1730867739.55, time is 1730867738.79 2024-11-06T04:35:39Z- AHRS_M2 msg: 1730867739.15 epoch sec; seqNo:50 2024-11-06T04:35:39Z- AHRS_M2 msg: 0.10 phi; -0.05 theta; 5.51 psi 2024-11-06T04:35:39Z- handleDepth()- Depth_Keller msg: 1730867739.16 epoch sec; seqNo:55; depth 4.70 2024-11-06T04:35:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867739.19 epoch sec; seqNo:55; 4061487.20 north; 594466.98 east; 36.69 lat; -121.94 lon; 4.70 depth; utmZone: 10 2024-11-06T04:35:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867735.55 2024-11-06T04:35:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:39Z- waiting for 1730867739.55, time is 1730867739.19 2024-11-06T04:35:39Z- AHRS_M2 msg: 1730867739.56 epoch sec; seqNo:51 2024-11-06T04:35:39Z- AHRS_M2 msg: 0.09 phi; -0.02 theta; 5.51 psi 2024-11-06T04:35:39Z- handleDepth()- Depth_Keller msg: 1730867739.56 epoch sec; seqNo:56; depth 4.75 2024-11-06T04:35:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867739.57 epoch sec; seqNo:56; 4061487.58 north; 594466.98 east; 36.69 lat; -121.94 lon; 4.75 depth; utmZone: 10 2024-11-06T04:35:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867735.55 2024-11-06T04:35:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:39Z- waiting for 1730867739.55, time is 1730867739.57 2024-11-06T04:35:39Z- LcmTrn::updateTrn() >>>> pose time 1730867739.57, meas time 1730867738.44 <<<< 2024-11-06T04:35:39Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867739.57 2024-11-06T04:35:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867738.44 sec, ping # 0; waiting for INS data more recent than 1730867735.55... 2024-11-06T04:35:39Z- TerrainNav::motionUpdate() - currEstimate = (4061488, 594466.98, 4.75) 2024-11-06T04:35:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:35:39Z- TerrainNav:: SubcloudNIS is 0.08, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:35:39Z- TNavPF::Projecting Measurements 2024-11-06T04:35:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:35:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:35:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:35:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:35:39Z- TNavFilter::4 good beams. 2024-11-06T04:35:39Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:35:39Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:35:39Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:35:39Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:35:39Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:35:39Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:35:39Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:35:39Z- TerrainMapOctree: (vehN, vehE ) = (4061486.74, 594468.02). 2024-11-06T04:35:39Z- TerrainMapOctree: Min Distance = 321.84. 2024-11-06T04:35:39Z- TerrainMapOctree: Using tile 1. 2024-11-06T04:35:39Z- Forcing Subcloud Comparison 2024-11-06T04:35:39Z- Weighting particles with subcloud comparison 2024-11-06T04:35:39Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:39Z- meanExpectedMeasDiff: 1.277720 delta_rms_squared: 2.353549 alpha: 0.991574 2024-11-06T04:35:39Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:39Z- meanExpectedMeasDiff: -3.504306 delta_rms_squared: 4.596192 alpha: 0.995667 2024-11-06T04:35:39Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:39Z- meanExpectedMeasDiff: -6.494447 delta_rms_squared: 2.442174 alpha: 0.991877 2024-11-06T04:35:39Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:39Z- meanExpectedMeasDiff: -0.322253 delta_rms_squared: 2.897837 alpha: 0.993146 2024-11-06T04:35:39Z- Subcloud NIS: 0.084804 2024-11-06T04:35:39Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:35:39Z- TNavPF:: sumWeights = 0.125339 2024-11-06T04:35:39Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:35:39Z- TNavPF::First Term of Map Covariance Matrix is 0.52 2024-11-06T04:35:39Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T04:35:39Z- TerrainNav::Resampling particle filter... 2024-11-06T04:35:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867738.44 sec. 2024-11-06T04:35:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:35:39Z- variances([0], [2], and psi_berg) = 331.377566 78.515063 0.000000 2024-11-06T04:35:39Z- publishEstimatesDecoder() - at 1730867739.57 sec 2024-11-06T04:35:39Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:35:39Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061541.26 594499.42 9.43 2024-11-06T04:35:39Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061521.76 594515.48 11.26 lat/lon: 36.69 -121.94 2024-11-06T04:35:39Z- LcmTrn::publishEstimatesDecoder() - COVAR: 331.38 78.52 1.00 0.00 2024-11-06T04:35:39Z- AHRS_M2 msg: 1730867739.96 epoch sec; seqNo:52 2024-11-06T04:35:39Z- AHRS_M2 msg: 0.10 phi; -0.00 theta; 5.52 psi 2024-11-06T04:35:39Z- handleDepth()- Depth_Keller msg: 1730867739.97 epoch sec; seqNo:57; depth 4.80 2024-11-06T04:35:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867739.99 epoch sec; seqNo:57; 4061487.95 north; 594465.75 east; 36.69 lat; -121.94 lon; 4.80 depth; utmZone: 10 2024-11-06T04:35:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867739.57 2024-11-06T04:35:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:40Z- waiting for 1730867743.57, time is 1730867739.99 2024-11-06T04:35:40Z- handleDepth()- Depth_Keller msg: 1730867740.44 epoch sec; seqNo:58; depth 4.85 2024-11-06T04:35:40Z- handleDVL() - DVL msg epochMillisec: 1730867740.46 2024-11-06T04:35:40Z- handleDvl() - RDI_Pathfinder msg: 1730867740.46 sec; seqNo:17; #beams:4 2024-11-06T04:35:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.11 , 77.69 , 87.24 , 83.59 2024-11-06T04:35:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.22 , 0.62 2024-11-06T04:35:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867740.49 epoch sec; seqNo:58; 4061487.95 north; 594465.75 east; 36.69 lat; -121.94 lon; 4.85 depth; utmZone: 10 2024-11-06T04:35:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867739.57 2024-11-06T04:35:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:40Z- waiting for 1730867743.57, time is 1730867740.49 2024-11-06T04:35:40Z- handleDepth()- Depth_Keller msg: 1730867740.78 epoch sec; seqNo:59; depth 4.89 2024-11-06T04:35:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867740.80 epoch sec; seqNo:59; 4061488.33 north; 594465.75 east; 36.69 lat; -121.94 lon; 4.89 depth; utmZone: 10 2024-11-06T04:35:40Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:35:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867739.57 2024-11-06T04:35:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:40Z- waiting for 1730867743.57, time is 1730867740.80 2024-11-06T04:35:41Z- AHRS_M2 msg: 1730867741.18 epoch sec; seqNo:53 2024-11-06T04:35:41Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 5.52 psi 2024-11-06T04:35:41Z- handleDepth()- Depth_Keller msg: 1730867741.19 epoch sec; seqNo:60; depth 4.89 2024-11-06T04:35:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867741.20 epoch sec; seqNo:60; 4061488.33 north; 594465.75 east; 36.69 lat; -121.94 lon; 4.89 depth; utmZone: 10 2024-11-06T04:35:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867739.57 2024-11-06T04:35:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:41Z- waiting for 1730867743.57, time is 1730867741.20 2024-11-06T04:35:41Z- AHRS_M2 msg: 1730867741.58 epoch sec; seqNo:54 2024-11-06T04:35:41Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 5.53 psi 2024-11-06T04:35:41Z- handleDepth()- Depth_Keller msg: 1730867741.59 epoch sec; seqNo:61; depth 4.89 2024-11-06T04:35:41Z- handleDVL() - DVL msg epochMillisec: 1730867741.59 2024-11-06T04:35:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:35:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:35:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:35:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:35:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:35:41Z- handleDvl() - RDI_Pathfinder msg: 1730867741.59 sec; seqNo:18; #beams:0 2024-11-06T04:35:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:35:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.22 , 0.62 2024-11-06T04:35:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867741.62 epoch sec; seqNo:61; 4061488.69 north; 594464.53 east; 36.69 lat; -121.94 lon; 4.89 depth; utmZone: 10 2024-11-06T04:35:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867739.57 2024-11-06T04:35:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:41Z- waiting for 1730867743.57, time is 1730867741.62 2024-11-06T04:35:41Z- AHRS_M2 msg: 1730867741.99 epoch sec; seqNo:55 2024-11-06T04:35:41Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 5.54 psi 2024-11-06T04:35:41Z- handleDepth()- Depth_Keller msg: 1730867741.99 epoch sec; seqNo:62; depth 4.88 2024-11-06T04:35:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867742.00 epoch sec; seqNo:62; 4061488.69 north; 594464.53 east; 36.69 lat; -121.94 lon; 4.88 depth; utmZone: 10 2024-11-06T04:35:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867739.57 2024-11-06T04:35:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:42Z- waiting for 1730867743.57, time is 1730867742.00 2024-11-06T04:35:42Z- handleDepth()- Depth_Keller msg: 1730867742.44 epoch sec; seqNo:63; depth 4.88 2024-11-06T04:35:42Z- handleDVL() - DVL msg epochMillisec: 1730867742.46 2024-11-06T04:35:42Z- handleDvl() - RDI_Pathfinder msg: 1730867742.46 sec; seqNo:19; #beams:4 2024-11-06T04:35:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.98 , 77.28 , 88.46 , 86.02 2024-11-06T04:35:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.31 , 0.09 , -0.03 2024-11-06T04:35:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867742.48 epoch sec; seqNo:63; 4061488.69 north; 594464.53 east; 36.69 lat; -121.94 lon; 4.88 depth; utmZone: 10 2024-11-06T04:35:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867739.57 2024-11-06T04:35:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:42Z- waiting for 1730867743.57, time is 1730867742.48 2024-11-06T04:35:42Z- handleDepth()- Depth_Keller msg: 1730867742.81 epoch sec; seqNo:64; depth 4.86 2024-11-06T04:35:42Z- handleDVL() - DVL msg epochMillisec: 1730867742.82 2024-11-06T04:35:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:35:42Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:35:42Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:35:42Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:35:42Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:35:42Z- handleDvl() - RDI_Pathfinder msg: 1730867742.82 sec; seqNo:20; #beams:0 2024-11-06T04:35:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:35:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.31 , 0.09 , -0.03 2024-11-06T04:35:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867742.84 epoch sec; seqNo:64; 4061489.07 north; 594464.52 east; 36.69 lat; -121.94 lon; 4.86 depth; utmZone: 10 2024-11-06T04:35:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867739.57 2024-11-06T04:35:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:42Z- waiting for 1730867743.57, time is 1730867742.84 2024-11-06T04:35:43Z- AHRS_M2 msg: 1730867743.19 epoch sec; seqNo:56 2024-11-06T04:35:43Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 5.56 psi 2024-11-06T04:35:43Z- handleDepth()- Depth_Keller msg: 1730867743.20 epoch sec; seqNo:65; depth 4.86 2024-11-06T04:35:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867743.22 epoch sec; seqNo:65; 4061489.07 north; 594464.52 east; 36.69 lat; -121.94 lon; 4.86 depth; utmZone: 10 2024-11-06T04:35:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867739.57 2024-11-06T04:35:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:43Z- waiting for 1730867743.57, time is 1730867743.22 2024-11-06T04:35:43Z- AHRS_M2 msg: 1730867743.61 epoch sec; seqNo:57 2024-11-06T04:35:43Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 5.56 psi 2024-11-06T04:35:43Z- handleDepth()- Depth_Keller msg: 1730867743.61 epoch sec; seqNo:66; depth 4.86 2024-11-06T04:35:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867743.62 epoch sec; seqNo:66; 4061489.07 north; 594464.52 east; 36.69 lat; -121.94 lon; 4.86 depth; utmZone: 10 2024-11-06T04:35:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867739.57 2024-11-06T04:35:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:43Z- waiting for 1730867743.57, time is 1730867743.62 2024-11-06T04:35:43Z- LcmTrn::updateTrn() >>>> pose time 1730867743.62, meas time 1730867742.82 <<<< 2024-11-06T04:35:43Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867743.62 2024-11-06T04:35:43Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:35:43Z- TerrainNav::motionUpdate() - currEstimate = (4061489, 594464.52, 4.86) 2024-11-06T04:35:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:35:43Z- TerrainNav:: SubcloudNIS is 0.08, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:35:43Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:35:43Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:35:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:35:43Z- variances([0], [2], and psi_berg) = 331.382360 78.508118 0.000000 2024-11-06T04:35:43Z- publishEstimatesDecoder() - at 1730867743.62 sec 2024-11-06T04:35:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:35:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061542.69 594497.01 9.76 2024-11-06T04:35:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061523.25 594513.02 11.37 lat/lon: 36.69 -121.94 2024-11-06T04:35:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 331.38 78.51 1.24 0.00 2024-11-06T04:35:44Z- AHRS_M2 msg: 1730867744.00 epoch sec; seqNo:58 2024-11-06T04:35:44Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 5.57 psi 2024-11-06T04:35:44Z- handleDepth()- Depth_Keller msg: 1730867744.02 epoch sec; seqNo:67; depth 4.85 2024-11-06T04:35:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867744.03 epoch sec; seqNo:67; 4061489.45 north; 594464.52 east; 36.69 lat; -121.94 lon; 4.85 depth; utmZone: 10 2024-11-06T04:35:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867743.62 2024-11-06T04:35:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:44Z- waiting for 1730867747.62, time is 1730867744.03 2024-11-06T04:35:44Z- AHRS_M2 msg: 1730867744.43 epoch sec; seqNo:59 2024-11-06T04:35:44Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 5.56 psi 2024-11-06T04:35:44Z- handleDepth()- Depth_Keller msg: 1730867744.44 epoch sec; seqNo:68; depth 4.83 2024-11-06T04:35:44Z- handleDVL() - DVL msg epochMillisec: 1730867744.46 2024-11-06T04:35:44Z- handleDvl() - RDI_Pathfinder msg: 1730867744.46 sec; seqNo:21; #beams:4 2024-11-06T04:35:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 95.56 , 76.87 , 85.82 , 87.04 2024-11-06T04:35:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.07 , -0.57 2024-11-06T04:35:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867744.47 epoch sec; seqNo:68; 4061489.45 north; 594464.52 east; 36.69 lat; -121.94 lon; 4.83 depth; utmZone: 10 2024-11-06T04:35:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867743.62 2024-11-06T04:35:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:44Z- waiting for 1730867747.62, time is 1730867744.47 2024-11-06T04:35:44Z- handleDepth()- Depth_Keller msg: 1730867744.84 epoch sec; seqNo:69; depth 4.84 2024-11-06T04:35:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867744.86 epoch sec; seqNo:69; 4061489.82 north; 594463.29 east; 36.69 lat; -121.94 lon; 4.84 depth; utmZone: 10 2024-11-06T04:35:44Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:35:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867743.62 2024-11-06T04:35:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:44Z- waiting for 1730867747.62, time is 1730867744.86 2024-11-06T04:35:45Z- AHRS_M2 msg: 1730867745.22 epoch sec; seqNo:60 2024-11-06T04:35:45Z- AHRS_M2 msg: 0.11 phi; -0.02 theta; 5.55 psi 2024-11-06T04:35:45Z- handleDepth()- Depth_Keller msg: 1730867745.22 epoch sec; seqNo:70; depth 4.85 2024-11-06T04:35:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867745.23 epoch sec; seqNo:70; 4061490.19 north; 594463.29 east; 36.69 lat; -121.94 lon; 4.85 depth; utmZone: 10 2024-11-06T04:35:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867743.62 2024-11-06T04:35:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:45Z- waiting for 1730867747.62, time is 1730867745.23 2024-11-06T04:35:45Z- AHRS_M2 msg: 1730867745.62 epoch sec; seqNo:61 2024-11-06T04:35:45Z- AHRS_M2 msg: 0.11 phi; -0.02 theta; 5.55 psi 2024-11-06T04:35:45Z- handleDepth()- Depth_Keller msg: 1730867745.63 epoch sec; seqNo:71; depth 4.83 2024-11-06T04:35:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867745.64 epoch sec; seqNo:71; 4061490.19 north; 594463.29 east; 36.69 lat; -121.94 lon; 4.83 depth; utmZone: 10 2024-11-06T04:35:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867743.62 2024-11-06T04:35:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:45Z- waiting for 1730867747.62, time is 1730867745.64 2024-11-06T04:35:46Z- AHRS_M2 msg: 1730867746.02 epoch sec; seqNo:62 2024-11-06T04:35:46Z- AHRS_M2 msg: 0.11 phi; -0.02 theta; 5.54 psi 2024-11-06T04:35:46Z- handleDepth()- Depth_Keller msg: 1730867746.03 epoch sec; seqNo:72; depth 4.84 2024-11-06T04:35:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867746.04 epoch sec; seqNo:72; 4061490.57 north; 594463.28 east; 36.69 lat; -121.94 lon; 4.84 depth; utmZone: 10 2024-11-06T04:35:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867743.62 2024-11-06T04:35:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:46Z- waiting for 1730867747.62, time is 1730867746.04 2024-11-06T04:35:46Z- AHRS_M2 msg: 1730867746.43 epoch sec; seqNo:63 2024-11-06T04:35:46Z- AHRS_M2 msg: 0.11 phi; -0.04 theta; 5.53 psi 2024-11-06T04:35:46Z- handleDepth()- Depth_Keller msg: 1730867746.44 epoch sec; seqNo:73; depth 4.85 2024-11-06T04:35:46Z- handleDVL() - DVL msg epochMillisec: 1730867746.47 2024-11-06T04:35:46Z- handleDvl() - RDI_Pathfinder msg: 1730867746.47 sec; seqNo:22; #beams:4 2024-11-06T04:35:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.26 , 76.87 , 85.96 , 85.96 2024-11-06T04:35:46Z- handleDvl() - RDI_Pathfinder msg: velocities 1.17 , -0.16 , -0.25 2024-11-06T04:35:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867746.48 epoch sec; seqNo:73; 4061490.95 north; 594463.28 east; 36.69 lat; -121.94 lon; 4.85 depth; utmZone: 10 2024-11-06T04:35:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867743.62 2024-11-06T04:35:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:46Z- waiting for 1730867747.62, time is 1730867746.48 2024-11-06T04:35:46Z- AHRS_M2 msg: 1730867746.84 epoch sec; seqNo:64 2024-11-06T04:35:46Z- AHRS_M2 msg: 0.11 phi; -0.05 theta; 5.53 psi 2024-11-06T04:35:46Z- handleDepth()- Depth_Keller msg: 1730867746.85 epoch sec; seqNo:74; depth 4.86 2024-11-06T04:35:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867746.87 epoch sec; seqNo:74; 4061491.32 north; 594462.06 east; 36.69 lat; -121.94 lon; 4.86 depth; utmZone: 10 2024-11-06T04:35:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867743.62 2024-11-06T04:35:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:47Z- waiting for 1730867747.62, time is 1730867746.87 2024-11-06T04:35:47Z- AHRS_M2 msg: 1730867747.24 epoch sec; seqNo:65 2024-11-06T04:35:47Z- AHRS_M2 msg: 0.11 phi; -0.05 theta; 5.53 psi 2024-11-06T04:35:47Z- handleDepth()- Depth_Keller msg: 1730867747.24 epoch sec; seqNo:75; depth 4.88 2024-11-06T04:35:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867747.26 epoch sec; seqNo:75; 4061491.70 north; 594462.05 east; 36.69 lat; -121.94 lon; 4.88 depth; utmZone: 10 2024-11-06T04:35:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867743.62 2024-11-06T04:35:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:47Z- waiting for 1730867747.62, time is 1730867747.26 2024-11-06T04:35:47Z- AHRS_M2 msg: 1730867747.64 epoch sec; seqNo:66 2024-11-06T04:35:47Z- AHRS_M2 msg: 0.11 phi; -0.05 theta; 5.53 psi 2024-11-06T04:35:47Z- handleDepth()- Depth_Keller msg: 1730867747.64 epoch sec; seqNo:76; depth 4.88 2024-11-06T04:35:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867747.66 epoch sec; seqNo:76; 4061491.70 north; 594462.05 east; 36.69 lat; -121.94 lon; 4.88 depth; utmZone: 10 2024-11-06T04:35:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867743.62 2024-11-06T04:35:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:47Z- waiting for 1730867747.62, time is 1730867747.66 2024-11-06T04:35:47Z- LcmTrn::updateTrn() >>>> pose time 1730867747.66, meas time 1730867746.47 <<<< 2024-11-06T04:35:47Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867747.66 2024-11-06T04:35:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867746.47 sec, ping # 0; waiting for INS data more recent than 1730867743.62... 2024-11-06T04:35:47Z- TerrainNav::motionUpdate() - currEstimate = (4061492, 594462.05, 4.88) 2024-11-06T04:35:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:35:47Z- TerrainNav:: SubcloudNIS is 0.08, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:35:47Z- TNavPF::Projecting Measurements 2024-11-06T04:35:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:35:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:35:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:35:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:35:47Z- TNavFilter::4 good beams. 2024-11-06T04:35:47Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:35:47Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:35:47Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:35:47Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:35:47Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:35:47Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:35:47Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:35:47Z- TerrainMapOctree: (vehN, vehE ) = (4061490.92, 594462.78). 2024-11-06T04:35:47Z- TerrainMapOctree: Min Distance = 321.43. 2024-11-06T04:35:47Z- TerrainMapOctree: Using tile 1. 2024-11-06T04:35:47Z- Forcing Subcloud Comparison 2024-11-06T04:35:47Z- Weighting particles with subcloud comparison 2024-11-06T04:35:47Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:47Z- meanExpectedMeasDiff: -2.259143 delta_rms_squared: 2.727572 alpha: 0.992721 2024-11-06T04:35:47Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:47Z- meanExpectedMeasDiff: -0.242403 delta_rms_squared: 3.999539 alpha: 0.995024 2024-11-06T04:35:47Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:47Z- meanExpectedMeasDiff: 0.859855 delta_rms_squared: 2.234355 alpha: 0.991128 2024-11-06T04:35:47Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:47Z- meanExpectedMeasDiff: -3.362500 delta_rms_squared: 2.514671 alpha: 0.992109 2024-11-06T04:35:47Z- Subcloud NIS: 0.079372 2024-11-06T04:35:47Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:35:47Z- TNavPF:: sumWeights = 0.421796 2024-11-06T04:35:47Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:35:47Z- TNavPF::First Term of Map Covariance Matrix is 1.16 2024-11-06T04:35:47Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T04:35:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867746.47 sec. 2024-11-06T04:35:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:35:47Z- variances([0], [2], and psi_berg) = 320.692315 60.327634 0.000000 2024-11-06T04:35:47Z- publishEstimatesDecoder() - at 1730867747.66 sec 2024-11-06T04:35:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:35:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061499.97 594515.12 13.29 2024-11-06T04:35:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061527.00 594511.70 11.75 lat/lon: 36.69 -121.94 2024-11-06T04:35:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 320.69 60.33 1.24 0.00 2024-11-06T04:35:48Z- AHRS_M2 msg: 1730867748.07 epoch sec; seqNo:67 2024-11-06T04:35:48Z- AHRS_M2 msg: 0.11 phi; -0.05 theta; 5.52 psi 2024-11-06T04:35:48Z- handleDepth()- Depth_Keller msg: 1730867748.07 epoch sec; seqNo:77; depth 4.89 2024-11-06T04:35:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867748.10 epoch sec; seqNo:77; 4061492.07 north; 594462.05 east; 36.69 lat; -121.94 lon; 4.89 depth; utmZone: 10 2024-11-06T04:35:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867747.66 2024-11-06T04:35:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:48Z- waiting for 1730867751.66, time is 1730867748.10 2024-11-06T04:35:48Z- AHRS_M2 msg: 1730867748.45 epoch sec; seqNo:68 2024-11-06T04:35:48Z- AHRS_M2 msg: 0.11 phi; -0.04 theta; 5.52 psi 2024-11-06T04:35:48Z- handleDepth()- Depth_Keller msg: 1730867748.46 epoch sec; seqNo:78; depth 4.90 2024-11-06T04:35:48Z- handleDVL() - DVL msg epochMillisec: 1730867748.48 2024-11-06T04:35:48Z- handleDvl() - RDI_Pathfinder msg: 1730867748.48 sec; seqNo:23; #beams:4 2024-11-06T04:35:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.33 , 76.46 , 83.59 , 87.24 2024-11-06T04:35:48Z- handleDvl() - RDI_Pathfinder msg: velocities 1.30 , -0.23 , -0.09 2024-11-06T04:35:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867748.49 epoch sec; seqNo:78; 4061492.44 north; 594460.82 east; 36.69 lat; -121.94 lon; 4.90 depth; utmZone: 10 2024-11-06T04:35:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867747.66 2024-11-06T04:35:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:48Z- waiting for 1730867751.66, time is 1730867748.49 2024-11-06T04:35:48Z- AHRS_M2 msg: 1730867748.86 epoch sec; seqNo:69 2024-11-06T04:35:48Z- AHRS_M2 msg: 0.10 phi; -0.04 theta; 5.51 psi 2024-11-06T04:35:48Z- handleDepth()- Depth_Keller msg: 1730867748.87 epoch sec; seqNo:79; depth 4.92 2024-11-06T04:35:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867748.89 epoch sec; seqNo:79; 4061492.82 north; 594460.82 east; 36.69 lat; -121.94 lon; 4.92 depth; utmZone: 10 2024-11-06T04:35:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867747.66 2024-11-06T04:35:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:49Z- waiting for 1730867751.66, time is 1730867748.89 2024-11-06T04:35:49Z- AHRS_M2 msg: 1730867749.25 epoch sec; seqNo:70 2024-11-06T04:35:49Z- AHRS_M2 msg: 0.10 phi; -0.03 theta; 5.51 psi 2024-11-06T04:35:49Z- handleDepth()- Depth_Keller msg: 1730867749.27 epoch sec; seqNo:80; depth 4.95 2024-11-06T04:35:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867749.28 epoch sec; seqNo:80; 4061493.20 north; 594460.81 east; 36.69 lat; -121.94 lon; 4.95 depth; utmZone: 10 2024-11-06T04:35:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867747.66 2024-11-06T04:35:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:49Z- waiting for 1730867751.66, time is 1730867749.28 2024-11-06T04:35:49Z- AHRS_M2 msg: 1730867749.66 epoch sec; seqNo:71 2024-11-06T04:35:49Z- AHRS_M2 msg: 0.10 phi; -0.04 theta; 5.50 psi 2024-11-06T04:35:49Z- handleDepth()- Depth_Keller msg: 1730867749.66 epoch sec; seqNo:81; depth 4.96 2024-11-06T04:35:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867749.68 epoch sec; seqNo:81; 4061493.56 north; 594459.59 east; 36.69 lat; -121.94 lon; 4.96 depth; utmZone: 10 2024-11-06T04:35:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867747.66 2024-11-06T04:35:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:49Z- waiting for 1730867751.66, time is 1730867749.68 2024-11-06T04:35:50Z- AHRS_M2 msg: 1730867750.07 epoch sec; seqNo:72 2024-11-06T04:35:50Z- AHRS_M2 msg: 0.10 phi; -0.04 theta; 5.50 psi 2024-11-06T04:35:50Z- handleDepth()- Depth_Keller msg: 1730867750.08 epoch sec; seqNo:82; depth 4.99 2024-11-06T04:35:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867750.10 epoch sec; seqNo:82; 4061493.56 north; 594459.59 east; 36.69 lat; -121.94 lon; 4.99 depth; utmZone: 10 2024-11-06T04:35:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867747.66 2024-11-06T04:35:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:50Z- waiting for 1730867751.66, time is 1730867750.10 2024-11-06T04:35:50Z- AHRS_M2 msg: 1730867750.47 epoch sec; seqNo:73 2024-11-06T04:35:50Z- AHRS_M2 msg: 0.10 phi; -0.05 theta; 5.50 psi 2024-11-06T04:35:50Z- handleDepth()- Depth_Keller msg: 1730867750.47 epoch sec; seqNo:83; depth 5.00 2024-11-06T04:35:50Z- handleDVL() - DVL msg epochMillisec: 1730867750.49 2024-11-06T04:35:50Z- handleDvl() - RDI_Pathfinder msg: 1730867750.49 sec; seqNo:24; #beams:4 2024-11-06T04:35:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.33 , 77.28 , 84.81 , 86.02 2024-11-06T04:35:50Z- handleDvl() - RDI_Pathfinder msg: velocities 1.13 , -0.29 , 0.50 2024-11-06T04:35:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867750.53 epoch sec; seqNo:83; 4061493.93 north; 594458.37 east; 36.69 lat; -121.94 lon; 5.00 depth; utmZone: 10 2024-11-06T04:35:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867747.66 2024-11-06T04:35:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:50Z- waiting for 1730867751.66, time is 1730867750.53 2024-11-06T04:35:50Z- AHRS_M2 msg: 1730867750.88 epoch sec; seqNo:74 2024-11-06T04:35:50Z- AHRS_M2 msg: 0.10 phi; -0.06 theta; 5.49 psi 2024-11-06T04:35:50Z- handleDepth()- Depth_Keller msg: 1730867750.89 epoch sec; seqNo:84; depth 5.04 2024-11-06T04:35:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867750.91 epoch sec; seqNo:84; 4061494.31 north; 594458.36 east; 36.69 lat; -121.94 lon; 5.04 depth; utmZone: 10 2024-11-06T04:35:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867747.66 2024-11-06T04:35:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:51Z- waiting for 1730867751.66, time is 1730867750.91 2024-11-06T04:35:51Z- AHRS_M2 msg: 1730867751.28 epoch sec; seqNo:75 2024-11-06T04:35:51Z- AHRS_M2 msg: 0.11 phi; -0.07 theta; 5.49 psi 2024-11-06T04:35:51Z- handleDepth()- Depth_Keller msg: 1730867751.28 epoch sec; seqNo:85; depth 5.06 2024-11-06T04:35:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867751.29 epoch sec; seqNo:85; 4061494.31 north; 594458.36 east; 36.69 lat; -121.94 lon; 5.06 depth; utmZone: 10 2024-11-06T04:35:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867747.66 2024-11-06T04:35:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:51Z- waiting for 1730867751.66, time is 1730867751.29 2024-11-06T04:35:51Z- AHRS_M2 msg: 1730867751.70 epoch sec; seqNo:76 2024-11-06T04:35:51Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 5.49 psi 2024-11-06T04:35:51Z- handleDepth()- Depth_Keller msg: 1730867751.73 epoch sec; seqNo:86; depth 5.09 2024-11-06T04:35:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867751.74 epoch sec; seqNo:86; 4061494.67 north; 594457.14 east; 36.69 lat; -121.94 lon; 5.09 depth; utmZone: 10 2024-11-06T04:35:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867747.66 2024-11-06T04:35:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:51Z- waiting for 1730867751.66, time is 1730867751.74 2024-11-06T04:35:51Z- LcmTrn::updateTrn() >>>> pose time 1730867751.74, meas time 1730867750.49 <<<< 2024-11-06T04:35:51Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867751.74 2024-11-06T04:35:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867750.49 sec, ping # 0; waiting for INS data more recent than 1730867747.66... 2024-11-06T04:35:51Z- TerrainNav::motionUpdate() - currEstimate = (4061495, 594457.14, 5.09) 2024-11-06T04:35:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:35:51Z- TerrainNav:: SubcloudNIS is 0.08, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:35:51Z- TNavPF::Projecting Measurements 2024-11-06T04:35:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:35:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:35:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:35:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:35:51Z- TNavFilter::4 good beams. 2024-11-06T04:35:51Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:35:51Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:35:51Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:35:51Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:35:51Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:35:51Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:35:51Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:35:51Z- TerrainMapOctree: (vehN, vehE ) = (4061493.76, 594458.64). 2024-11-06T04:35:51Z- TerrainMapOctree: Min Distance = 320.86. 2024-11-06T04:35:51Z- TerrainMapOctree: Using tile 1. 2024-11-06T04:35:52Z- Forcing Subcloud Comparison 2024-11-06T04:35:52Z- Weighting particles with subcloud comparison 2024-11-06T04:35:52Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:52Z- meanExpectedMeasDiff: -1.623784 delta_rms_squared: 2.553753 alpha: 0.992229 2024-11-06T04:35:52Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:52Z- meanExpectedMeasDiff: 0.771194 delta_rms_squared: 4.473136 alpha: 0.995549 2024-11-06T04:35:52Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:52Z- meanExpectedMeasDiff: 1.305796 delta_rms_squared: 2.071026 alpha: 0.990435 2024-11-06T04:35:52Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:52Z- meanExpectedMeasDiff: -3.938540 delta_rms_squared: 2.322563 alpha: 0.991462 2024-11-06T04:35:52Z- Subcloud NIS: 0.090139 2024-11-06T04:35:52Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:35:52Z- TNavPF:: sumWeights = 0.354644 2024-11-06T04:35:52Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:35:52Z- TNavPF::First Term of Map Covariance Matrix is 0.83 2024-11-06T04:35:52Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T04:35:52Z- TerrainNav::Resampling particle filter... 2024-11-06T04:35:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867750.49 sec. 2024-11-06T04:35:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:35:52Z- variances([0], [2], and psi_berg) = 373.068817 51.336752 0.000000 2024-11-06T04:35:52Z- publishEstimatesDecoder() - at 1730867751.74 sec 2024-11-06T04:35:52Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:35:52Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061444.86 594510.91 10.91 2024-11-06T04:35:52Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061529.99 594507.00 12.25 lat/lon: 36.69 -121.94 2024-11-06T04:35:52Z- LcmTrn::publishEstimatesDecoder() - COVAR: 373.07 51.34 1.29 0.00 2024-11-06T04:35:52Z- AHRS_M2 msg: 1730867752.10 epoch sec; seqNo:77 2024-11-06T04:35:52Z- AHRS_M2 msg: 0.12 phi; -0.09 theta; 5.48 psi 2024-11-06T04:35:52Z- handleDepth()- Depth_Keller msg: 1730867752.13 epoch sec; seqNo:87; depth 5.11 2024-11-06T04:35:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867752.16 epoch sec; seqNo:87; 4061494.67 north; 594457.14 east; 36.69 lat; -121.94 lon; 5.11 depth; utmZone: 10 2024-11-06T04:35:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867751.74 2024-11-06T04:35:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:52Z- waiting for 1730867755.74, time is 1730867752.16 2024-11-06T04:35:52Z- AHRS_M2 msg: 1730867752.49 epoch sec; seqNo:78 2024-11-06T04:35:52Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 5.48 psi 2024-11-06T04:35:52Z- handleDepth()- Depth_Keller msg: 1730867752.50 epoch sec; seqNo:88; depth 5.12 2024-11-06T04:35:52Z- handleDVL() - DVL msg epochMillisec: 1730867752.53 2024-11-06T04:35:52Z- handleDvl() - RDI_Pathfinder msg: 1730867752.53 sec; seqNo:25; #beams:4 2024-11-06T04:35:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.55 , 76.87 , 84.81 , 89.68 2024-11-06T04:35:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.56 , -0.08 , 0.36 2024-11-06T04:35:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867752.54 epoch sec; seqNo:88; 4061495.05 north; 594457.13 east; 36.69 lat; -121.94 lon; 5.12 depth; utmZone: 10 2024-11-06T04:35:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867751.74 2024-11-06T04:35:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:52Z- waiting for 1730867755.74, time is 1730867752.54 2024-11-06T04:35:52Z- AHRS_M2 msg: 1730867752.89 epoch sec; seqNo:79 2024-11-06T04:35:52Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.48 psi 2024-11-06T04:35:52Z- handleDepth()- Depth_Keller msg: 1730867752.90 epoch sec; seqNo:89; depth 5.15 2024-11-06T04:35:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867752.93 epoch sec; seqNo:89; 4061495.05 north; 594457.13 east; 36.69 lat; -121.94 lon; 5.15 depth; utmZone: 10 2024-11-06T04:35:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867751.74 2024-11-06T04:35:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:53Z- waiting for 1730867755.74, time is 1730867752.93 2024-11-06T04:35:53Z- AHRS_M2 msg: 1730867753.30 epoch sec; seqNo:80 2024-11-06T04:35:53Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.48 psi 2024-11-06T04:35:53Z- handleDepth()- Depth_Keller msg: 1730867753.30 epoch sec; seqNo:90; depth 5.18 2024-11-06T04:35:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867753.32 epoch sec; seqNo:90; 4061495.05 north; 594457.13 east; 36.69 lat; -121.94 lon; 5.18 depth; utmZone: 10 2024-11-06T04:35:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867751.74 2024-11-06T04:35:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:53Z- waiting for 1730867755.74, time is 1730867753.32 2024-11-06T04:35:53Z- AHRS_M2 msg: 1730867753.70 epoch sec; seqNo:81 2024-11-06T04:35:53Z- AHRS_M2 msg: 0.14 phi; -0.14 theta; 5.48 psi 2024-11-06T04:35:53Z- handleDepth()- Depth_Keller msg: 1730867753.70 epoch sec; seqNo:91; depth 5.19 2024-11-06T04:35:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867753.72 epoch sec; seqNo:91; 4061495.04 north; 594455.91 east; 36.69 lat; -121.94 lon; 5.19 depth; utmZone: 10 2024-11-06T04:35:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867751.74 2024-11-06T04:35:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:53Z- waiting for 1730867755.74, time is 1730867753.72 2024-11-06T04:35:54Z- handleDepth()- Depth_Keller msg: 1730867754.13 epoch sec; seqNo:92; depth 5.23 2024-11-06T04:35:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867754.13 epoch sec; seqNo:92; 4061495.42 north; 594455.91 east; 36.69 lat; -121.94 lon; 5.23 depth; utmZone: 10 2024-11-06T04:35:54Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:35:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867751.74 2024-11-06T04:35:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:54Z- waiting for 1730867755.74, time is 1730867754.13 2024-11-06T04:35:54Z- AHRS_M2 msg: 1730867754.51 epoch sec; seqNo:82 2024-11-06T04:35:54Z- AHRS_M2 msg: 0.15 phi; -0.15 theta; 5.49 psi 2024-11-06T04:35:54Z- handleDepth()- Depth_Keller msg: 1730867754.52 epoch sec; seqNo:93; depth 5.24 2024-11-06T04:35:54Z- handleDVL() - DVL msg epochMillisec: 1730867754.54 2024-11-06T04:35:54Z- handleDvl() - RDI_Pathfinder msg: 1730867754.54 sec; seqNo:26; #beams:4 2024-11-06T04:35:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.85 , 76.87 , 79.73 , 91.98 2024-11-06T04:35:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.57 , 0.10 , -0.25 2024-11-06T04:35:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867754.59 epoch sec; seqNo:93; 4061495.42 north; 594455.91 east; 36.69 lat; -121.94 lon; 5.24 depth; utmZone: 10 2024-11-06T04:35:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867751.74 2024-11-06T04:35:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:54Z- waiting for 1730867755.74, time is 1730867754.59 2024-11-06T04:35:54Z- AHRS_M2 msg: 1730867754.98 epoch sec; seqNo:83 2024-11-06T04:35:54Z- AHRS_M2 msg: 0.13 phi; -0.14 theta; 5.49 psi 2024-11-06T04:35:54Z- handleDepth()- Depth_Keller msg: 1730867754.99 epoch sec; seqNo:94; depth 5.25 2024-11-06T04:35:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867755.01 epoch sec; seqNo:94; 4061495.80 north; 594455.90 east; 36.69 lat; -121.94 lon; 5.25 depth; utmZone: 10 2024-11-06T04:35:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867751.74 2024-11-06T04:35:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:55Z- waiting for 1730867755.74, time is 1730867755.01 2024-11-06T04:35:55Z- AHRS_M2 msg: 1730867755.38 epoch sec; seqNo:84 2024-11-06T04:35:55Z- AHRS_M2 msg: 0.12 phi; -0.14 theta; 5.49 psi 2024-11-06T04:35:55Z- handleDepth()- Depth_Keller msg: 1730867755.38 epoch sec; seqNo:95; depth 5.27 2024-11-06T04:35:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867755.41 epoch sec; seqNo:95; 4061495.80 north; 594455.90 east; 36.69 lat; -121.94 lon; 5.27 depth; utmZone: 10 2024-11-06T04:35:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867751.74 2024-11-06T04:35:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:55Z- waiting for 1730867755.74, time is 1730867755.41 2024-11-06T04:35:55Z- AHRS_M2 msg: 1730867755.80 epoch sec; seqNo:85 2024-11-06T04:35:55Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.49 psi 2024-11-06T04:35:55Z- handleDepth()- Depth_Keller msg: 1730867755.80 epoch sec; seqNo:96; depth 5.31 2024-11-06T04:35:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867755.81 epoch sec; seqNo:96; 4061496.17 north; 594455.90 east; 36.69 lat; -121.94 lon; 5.31 depth; utmZone: 10 2024-11-06T04:35:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867751.74 2024-11-06T04:35:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:55Z- waiting for 1730867755.74, time is 1730867755.81 2024-11-06T04:35:55Z- LcmTrn::updateTrn() >>>> pose time 1730867755.81, meas time 1730867754.54 <<<< 2024-11-06T04:35:55Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867755.81 2024-11-06T04:35:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867754.54 sec, ping # 0; waiting for INS data more recent than 1730867751.74... 2024-11-06T04:35:55Z- TerrainNav::motionUpdate() - currEstimate = (4061496, 594455.90, 5.31) 2024-11-06T04:35:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:35:55Z- TerrainNav:: SubcloudNIS is 0.09, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:35:55Z- TNavPF::Projecting Measurements 2024-11-06T04:35:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:35:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:35:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:35:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:35:55Z- TNavFilter::4 good beams. 2024-11-06T04:35:55Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:35:55Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:35:55Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:35:55Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:35:55Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:35:55Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:35:55Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:35:55Z- TerrainMapOctree: (vehN, vehE ) = (4061495.71, 594456.28). 2024-11-06T04:35:55Z- TerrainMapOctree: Min Distance = 320.82. 2024-11-06T04:35:55Z- TerrainMapOctree: Using tile 1. 2024-11-06T04:35:56Z- Forcing Subcloud Comparison 2024-11-06T04:35:56Z- Weighting particles with subcloud comparison 2024-11-06T04:35:56Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:56Z- meanExpectedMeasDiff: 2.033003 delta_rms_squared: 2.568518 alpha: 0.992274 2024-11-06T04:35:56Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:56Z- meanExpectedMeasDiff: 0.339074 delta_rms_squared: 3.781644 alpha: 0.994739 2024-11-06T04:35:56Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:56Z- meanExpectedMeasDiff: -1.682909 delta_rms_squared: 1.900166 alpha: 0.989584 2024-11-06T04:35:56Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:35:56Z- meanExpectedMeasDiff: -0.193979 delta_rms_squared: 2.680963 alpha: 0.992595 2024-11-06T04:35:56Z- Subcloud NIS: 0.095702 2024-11-06T04:35:56Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:35:56Z- TNavPF:: sumWeights = 0.563147 2024-11-06T04:35:56Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:35:56Z- TNavPF::First Term of Map Covariance Matrix is 1.40 2024-11-06T04:35:56Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T04:35:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867754.54 sec. 2024-11-06T04:35:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:35:56Z- variances([0], [2], and psi_berg) = 313.244377 47.710679 0.000000 2024-11-06T04:35:56Z- publishEstimatesDecoder() - at 1730867755.81 sec 2024-11-06T04:35:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:35:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061524.54 594494.35 12.56 2024-11-06T04:35:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061533.33 594506.05 12.45 lat/lon: 36.69 -121.94 2024-11-06T04:35:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 313.24 47.71 1.29 0.00 2024-11-06T04:35:56Z- AHRS_M2 msg: 1730867756.24 epoch sec; seqNo:86 2024-11-06T04:35:56Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.49 psi 2024-11-06T04:35:56Z- handleDepth()- Depth_Keller msg: 1730867756.27 epoch sec; seqNo:97; depth 5.35 2024-11-06T04:35:56Z- handleDVL() - DVL msg epochMillisec: 1730867756.27 2024-11-06T04:35:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:35:56Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:35:56Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:35:56Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:35:56Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:35:56Z- handleDvl() - RDI_Pathfinder msg: 1730867756.27 sec; seqNo:27; #beams:0 2024-11-06T04:35:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:35:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.57 , 0.10 , -0.25 2024-11-06T04:35:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867756.29 epoch sec; seqNo:97; 4061496.55 north; 594455.90 east; 36.69 lat; -121.94 lon; 5.35 depth; utmZone: 10 2024-11-06T04:35:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867755.81 2024-11-06T04:35:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:56Z- waiting for 1730867759.81, time is 1730867756.29 2024-11-06T04:35:56Z- AHRS_M2 msg: 1730867756.67 epoch sec; seqNo:87 2024-11-06T04:35:56Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.49 psi 2024-11-06T04:35:56Z- handleDepth()- Depth_Keller msg: 1730867756.68 epoch sec; seqNo:98; depth 5.40 2024-11-06T04:35:56Z- handleDVL() - DVL msg epochMillisec: 1730867756.70 2024-11-06T04:35:56Z- handleDvl() - RDI_Pathfinder msg: 1730867756.70 sec; seqNo:28; #beams:4 2024-11-06T04:35:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.41 , 77.28 , 77.69 , 90.76 2024-11-06T04:35:56Z- handleDvl() - RDI_Pathfinder msg: velocities 1.13 , -0.03 , -0.26 2024-11-06T04:35:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867756.74 epoch sec; seqNo:98; 4061496.92 north; 594454.67 east; 36.69 lat; -121.94 lon; 5.40 depth; utmZone: 10 2024-11-06T04:35:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867755.81 2024-11-06T04:35:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:56Z- waiting for 1730867759.81, time is 1730867756.74 2024-11-06T04:35:57Z- AHRS_M2 msg: 1730867757.09 epoch sec; seqNo:88 2024-11-06T04:35:57Z- AHRS_M2 msg: 0.13 phi; -0.14 theta; 5.48 psi 2024-11-06T04:35:57Z- handleDepth()- Depth_Keller msg: 1730867757.09 epoch sec; seqNo:99; depth 5.45 2024-11-06T04:35:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867757.11 epoch sec; seqNo:99; 4061496.92 north; 594454.67 east; 36.69 lat; -121.94 lon; 5.45 depth; utmZone: 10 2024-11-06T04:35:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867755.81 2024-11-06T04:35:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:57Z- waiting for 1730867759.81, time is 1730867757.11 2024-11-06T04:35:57Z- AHRS_M2 msg: 1730867757.49 epoch sec; seqNo:89 2024-11-06T04:35:57Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.47 psi 2024-11-06T04:35:57Z- handleDepth()- Depth_Keller msg: 1730867757.50 epoch sec; seqNo:100; depth 5.49 2024-11-06T04:35:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867757.51 epoch sec; seqNo:100; 4061497.30 north; 594454.67 east; 36.69 lat; -121.94 lon; 5.49 depth; utmZone: 10 2024-11-06T04:35:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867755.81 2024-11-06T04:35:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:57Z- waiting for 1730867759.81, time is 1730867757.51 2024-11-06T04:35:57Z- AHRS_M2 msg: 1730867757.90 epoch sec; seqNo:90 2024-11-06T04:35:57Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.47 psi 2024-11-06T04:35:57Z- handleDepth()- Depth_Keller msg: 1730867757.90 epoch sec; seqNo:101; depth 5.53 2024-11-06T04:35:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867757.92 epoch sec; seqNo:101; 4061497.68 north; 594454.66 east; 36.69 lat; -121.94 lon; 5.53 depth; utmZone: 10 2024-11-06T04:35:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867755.81 2024-11-06T04:35:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:58Z- waiting for 1730867759.81, time is 1730867757.92 2024-11-06T04:35:58Z- AHRS_M2 msg: 1730867758.30 epoch sec; seqNo:91 2024-11-06T04:35:58Z- AHRS_M2 msg: 0.12 phi; -0.16 theta; 5.46 psi 2024-11-06T04:35:58Z- handleDepth()- Depth_Keller msg: 1730867758.30 epoch sec; seqNo:102; depth 5.59 2024-11-06T04:35:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867758.31 epoch sec; seqNo:102; 4061498.04 north; 594453.44 east; 36.69 lat; -121.94 lon; 5.59 depth; utmZone: 10 2024-11-06T04:35:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867755.81 2024-11-06T04:35:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:58Z- waiting for 1730867759.81, time is 1730867758.31 2024-11-06T04:35:58Z- AHRS_M2 msg: 1730867758.71 epoch sec; seqNo:92 2024-11-06T04:35:58Z- AHRS_M2 msg: 0.12 phi; -0.17 theta; 5.47 psi 2024-11-06T04:35:58Z- handleDepth()- Depth_Keller msg: 1730867758.71 epoch sec; seqNo:103; depth 5.65 2024-11-06T04:35:58Z- handleDVL() - DVL msg epochMillisec: 1730867758.73 2024-11-06T04:35:58Z- handleDvl() - RDI_Pathfinder msg: 1730867758.73 sec; seqNo:29; #beams:4 2024-11-06T04:35:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.55 , 76.46 , 78.10 , 90.89 2024-11-06T04:35:58Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.12 , -0.00 2024-11-06T04:35:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867758.75 epoch sec; seqNo:103; 4061498.42 north; 594453.43 east; 36.69 lat; -121.94 lon; 5.65 depth; utmZone: 10 2024-11-06T04:35:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:35:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867755.81 2024-11-06T04:35:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:58Z- waiting for 1730867759.81, time is 1730867758.75 2024-11-06T04:35:59Z- AHRS_M2 msg: 1730867759.11 epoch sec; seqNo:93 2024-11-06T04:35:59Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.47 psi 2024-11-06T04:35:59Z- handleDepth()- Depth_Keller msg: 1730867759.12 epoch sec; seqNo:104; depth 5.68 2024-11-06T04:35:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867759.13 epoch sec; seqNo:104; 4061498.42 north; 594453.43 east; 36.69 lat; -121.94 lon; 5.68 depth; utmZone: 10 2024-11-06T04:35:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867755.81 2024-11-06T04:35:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:59Z- waiting for 1730867759.81, time is 1730867759.13 2024-11-06T04:35:59Z- AHRS_M2 msg: 1730867759.51 epoch sec; seqNo:94 2024-11-06T04:35:59Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.48 psi 2024-11-06T04:35:59Z- handleDepth()- Depth_Keller msg: 1730867759.51 epoch sec; seqNo:105; depth 5.73 2024-11-06T04:35:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867759.52 epoch sec; seqNo:105; 4061498.80 north; 594453.43 east; 36.69 lat; -121.94 lon; 5.73 depth; utmZone: 10 2024-11-06T04:35:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:35:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867755.81 2024-11-06T04:35:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:35:59Z- waiting for 1730867759.81, time is 1730867759.52 2024-11-06T04:35:59Z- AHRS_M2 msg: 1730867759.92 epoch sec; seqNo:95 2024-11-06T04:35:59Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.48 psi 2024-11-06T04:35:59Z- handleDepth()- Depth_Keller msg: 1730867759.92 epoch sec; seqNo:106; depth 5.78 2024-11-06T04:35:59Z- handleDVL() - DVL msg epochMillisec: 1730867759.93 2024-11-06T04:35:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:35:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:35:59Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:35:59Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:35:59Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:35:59Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:35:59Z- handleDvl() - RDI_Pathfinder msg: 1730867759.93 sec; seqNo:30; #beams:0 2024-11-06T04:35:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:35:59Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.12 , -0.00 2024-11-06T04:35:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867759.94 epoch sec; seqNo:106; 4061498.79 north; 594452.21 east; 36.69 lat; -121.94 lon; 5.78 depth; utmZone: 10 2024-11-06T04:36:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867755.81 2024-11-06T04:36:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:00Z- waiting for 1730867759.81, time is 1730867759.94 2024-11-06T04:36:00Z- LcmTrn::updateTrn() >>>> pose time 1730867759.94, meas time 1730867759.93 <<<< 2024-11-06T04:36:00Z- LcmTrn::updateTrn() equating pose 1730867759.93 and meas times 1730867759.93 2024-11-06T04:36:00Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867759.93 2024-11-06T04:36:00Z- TerrainNav::motionUpdate() - currEstimate = (4061499, 594452.21, 5.78) 2024-11-06T04:36:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:36:00Z- TerrainNav:: SubcloudNIS is 0.10, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:36:00Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:36:00Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:36:00Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:36:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:36:00Z- variances([0], [2], and psi_berg) = 313.221304 47.724521 0.000000 2024-11-06T04:36:00Z- publishEstimatesDecoder() - at 1730867759.93 sec 2024-11-06T04:36:00Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:36:00Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061527.13 594490.56 13.01 2024-11-06T04:36:00Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061535.94 594502.36 12.91 lat/lon: 36.69 -121.94 2024-11-06T04:36:00Z- LcmTrn::publishEstimatesDecoder() - COVAR: 313.22 47.72 1.54 0.00 2024-11-06T04:36:00Z- handleDepth()- Depth_Keller msg: 1730867760.33 epoch sec; seqNo:107; depth 5.82 2024-11-06T04:36:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867760.36 epoch sec; seqNo:107; 4061499.17 north; 594452.21 east; 36.69 lat; -121.94 lon; 5.82 depth; utmZone: 10 2024-11-06T04:36:00Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:36:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867759.93 2024-11-06T04:36:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:00Z- waiting for 1730867763.93, time is 1730867760.36 2024-11-06T04:36:00Z- AHRS_M2 msg: 1730867760.72 epoch sec; seqNo:96 2024-11-06T04:36:00Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.48 psi 2024-11-06T04:36:00Z- handleDepth()- Depth_Keller msg: 1730867760.73 epoch sec; seqNo:108; depth 5.87 2024-11-06T04:36:00Z- handleDVL() - DVL msg epochMillisec: 1730867760.76 2024-11-06T04:36:00Z- handleDvl() - RDI_Pathfinder msg: 1730867760.76 sec; seqNo:31; #beams:4 2024-11-06T04:36:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.33 , 76.87 , 76.87 , 93.33 2024-11-06T04:36:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.19 , -0.12 2024-11-06T04:36:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867760.77 epoch sec; seqNo:108; 4061499.54 north; 594452.20 east; 36.69 lat; -121.94 lon; 5.87 depth; utmZone: 10 2024-11-06T04:36:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867759.93 2024-11-06T04:36:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:00Z- waiting for 1730867763.93, time is 1730867760.77 2024-11-06T04:36:01Z- AHRS_M2 msg: 1730867761.12 epoch sec; seqNo:97 2024-11-06T04:36:01Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.48 psi 2024-11-06T04:36:01Z- handleDepth()- Depth_Keller msg: 1730867761.13 epoch sec; seqNo:109; depth 5.94 2024-11-06T04:36:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867761.14 epoch sec; seqNo:109; 4061499.53 north; 594450.98 east; 36.69 lat; -121.94 lon; 5.94 depth; utmZone: 10 2024-11-06T04:36:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867759.93 2024-11-06T04:36:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:01Z- waiting for 1730867763.93, time is 1730867761.14 2024-11-06T04:36:01Z- AHRS_M2 msg: 1730867761.53 epoch sec; seqNo:98 2024-11-06T04:36:01Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.48 psi 2024-11-06T04:36:01Z- handleDepth()- Depth_Keller msg: 1730867761.53 epoch sec; seqNo:110; depth 5.99 2024-11-06T04:36:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867761.54 epoch sec; seqNo:110; 4061499.91 north; 594450.98 east; 36.69 lat; -121.94 lon; 5.99 depth; utmZone: 10 2024-11-06T04:36:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867759.93 2024-11-06T04:36:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:01Z- waiting for 1730867763.93, time is 1730867761.54 2024-11-06T04:36:01Z- AHRS_M2 msg: 1730867761.93 epoch sec; seqNo:99 2024-11-06T04:36:01Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.47 psi 2024-11-06T04:36:01Z- handleDepth()- Depth_Keller msg: 1730867761.95 epoch sec; seqNo:111; depth 6.05 2024-11-06T04:36:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867761.97 epoch sec; seqNo:111; 4061500.29 north; 594450.97 east; 36.69 lat; -121.94 lon; 6.05 depth; utmZone: 10 2024-11-06T04:36:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867759.93 2024-11-06T04:36:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:02Z- waiting for 1730867763.93, time is 1730867761.97 2024-11-06T04:36:02Z- AHRS_M2 msg: 1730867762.34 epoch sec; seqNo:100 2024-11-06T04:36:02Z- AHRS_M2 msg: 0.13 phi; -0.18 theta; 5.48 psi 2024-11-06T04:36:02Z- handleDepth()- Depth_Keller msg: 1730867762.38 epoch sec; seqNo:112; depth 6.12 2024-11-06T04:36:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867762.41 epoch sec; seqNo:112; 4061500.29 north; 594450.97 east; 36.69 lat; -121.94 lon; 6.12 depth; utmZone: 10 2024-11-06T04:36:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867759.93 2024-11-06T04:36:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:02Z- waiting for 1730867763.93, time is 1730867762.41 2024-11-06T04:36:02Z- AHRS_M2 msg: 1730867762.74 epoch sec; seqNo:101 2024-11-06T04:36:02Z- AHRS_M2 msg: 0.13 phi; -0.17 theta; 5.48 psi 2024-11-06T04:36:02Z- handleDepth()- Depth_Keller msg: 1730867762.74 epoch sec; seqNo:113; depth 6.20 2024-11-06T04:36:02Z- handleDVL() - DVL msg epochMillisec: 1730867762.77 2024-11-06T04:36:02Z- handleDvl() - RDI_Pathfinder msg: 1730867762.77 sec; seqNo:32; #beams:4 2024-11-06T04:36:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.20 , 76.46 , 76.46 , 92.11 2024-11-06T04:36:02Z- handleDvl() - RDI_Pathfinder msg: velocities 1.13 , -0.29 , 0.02 2024-11-06T04:36:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867762.80 epoch sec; seqNo:113; 4061500.65 north; 594449.75 east; 36.69 lat; -121.94 lon; 6.20 depth; utmZone: 10 2024-11-06T04:36:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867759.93 2024-11-06T04:36:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:02Z- waiting for 1730867763.93, time is 1730867762.80 2024-11-06T04:36:03Z- AHRS_M2 msg: 1730867763.14 epoch sec; seqNo:102 2024-11-06T04:36:03Z- AHRS_M2 msg: 0.13 phi; -0.16 theta; 5.47 psi 2024-11-06T04:36:03Z- handleDepth()- Depth_Keller msg: 1730867763.15 epoch sec; seqNo:114; depth 6.26 2024-11-06T04:36:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867763.17 epoch sec; seqNo:114; 4061500.65 north; 594449.75 east; 36.69 lat; -121.94 lon; 6.26 depth; utmZone: 10 2024-11-06T04:36:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867759.93 2024-11-06T04:36:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:03Z- waiting for 1730867763.93, time is 1730867763.17 2024-11-06T04:36:03Z- AHRS_M2 msg: 1730867763.55 epoch sec; seqNo:103 2024-11-06T04:36:03Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.47 psi 2024-11-06T04:36:03Z- handleDepth()- Depth_Keller msg: 1730867763.56 epoch sec; seqNo:115; depth 6.32 2024-11-06T04:36:03Z- handleDVL() - DVL msg epochMillisec: 1730867763.56 2024-11-06T04:36:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:03Z- handleDvl() - RDI_Pathfinder msg: 1730867763.56 sec; seqNo:33; #beams:0 2024-11-06T04:36:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:03Z- handleDvl() - RDI_Pathfinder msg: velocities 1.13 , -0.29 , 0.02 2024-11-06T04:36:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867763.58 epoch sec; seqNo:115; 4061501.03 north; 594449.74 east; 36.69 lat; -121.94 lon; 6.32 depth; utmZone: 10 2024-11-06T04:36:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867759.93 2024-11-06T04:36:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:03Z- waiting for 1730867763.93, time is 1730867763.58 2024-11-06T04:36:03Z- AHRS_M2 msg: 1730867763.95 epoch sec; seqNo:104 2024-11-06T04:36:03Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.48 psi 2024-11-06T04:36:03Z- handleDepth()- Depth_Keller msg: 1730867763.95 epoch sec; seqNo:116; depth 6.39 2024-11-06T04:36:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867763.96 epoch sec; seqNo:116; 4061501.40 north; 594448.52 east; 36.69 lat; -121.94 lon; 6.39 depth; utmZone: 10 2024-11-06T04:36:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867759.93 2024-11-06T04:36:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:04Z- waiting for 1730867763.93, time is 1730867763.96 2024-11-06T04:36:04Z- LcmTrn::updateTrn() >>>> pose time 1730867763.96, meas time 1730867763.56 <<<< 2024-11-06T04:36:04Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867763.96 2024-11-06T04:36:04Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:36:04Z- TerrainNav::motionUpdate() - currEstimate = (4061501, 594448.52, 6.39) 2024-11-06T04:36:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:36:04Z- TerrainNav:: SubcloudNIS is 0.10, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:36:04Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:36:04Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:36:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:36:04Z- variances([0], [2], and psi_berg) = 313.200086 47.730478 0.000000 2024-11-06T04:36:04Z- publishEstimatesDecoder() - at 1730867763.96 sec 2024-11-06T04:36:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:36:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061529.78 594486.85 13.69 2024-11-06T04:36:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061538.56 594498.67 13.53 lat/lon: 36.69 -121.94 2024-11-06T04:36:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 313.20 47.73 1.78 0.00 2024-11-06T04:36:04Z- AHRS_M2 msg: 1730867764.39 epoch sec; seqNo:105 2024-11-06T04:36:04Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.48 psi 2024-11-06T04:36:04Z- handleDepth()- Depth_Keller msg: 1730867764.41 epoch sec; seqNo:117; depth 6.48 2024-11-06T04:36:04Z- handleDVL() - DVL msg epochMillisec: 1730867764.43 2024-11-06T04:36:04Z- handleDvl() - RDI_Pathfinder msg: 1730867764.43 sec; seqNo:34; #beams:4 2024-11-06T04:36:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.13 , 75.64 , 77.69 , 90.83 2024-11-06T04:36:04Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , -0.23 , 0.57 2024-11-06T04:36:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867764.45 epoch sec; seqNo:117; 4061501.40 north; 594448.52 east; 36.69 lat; -121.94 lon; 6.48 depth; utmZone: 10 2024-11-06T04:36:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867763.96 2024-11-06T04:36:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:04Z- waiting for 1730867767.96, time is 1730867764.45 2024-11-06T04:36:04Z- AHRS_M2 msg: 1730867764.76 epoch sec; seqNo:106 2024-11-06T04:36:04Z- AHRS_M2 msg: 0.13 phi; -0.12 theta; 5.49 psi 2024-11-06T04:36:04Z- handleDepth()- Depth_Keller msg: 1730867764.76 epoch sec; seqNo:118; depth 6.58 2024-11-06T04:36:04Z- handleDVL() - DVL msg epochMillisec: 1730867764.77 2024-11-06T04:36:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:04Z- handleDvl() - RDI_Pathfinder msg: 1730867764.77 sec; seqNo:35; #beams:0 2024-11-06T04:36:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:04Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , -0.23 , 0.57 2024-11-06T04:36:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867764.78 epoch sec; seqNo:118; 4061501.78 north; 594448.52 east; 36.69 lat; -121.94 lon; 6.58 depth; utmZone: 10 2024-11-06T04:36:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867763.96 2024-11-06T04:36:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:04Z- waiting for 1730867767.96, time is 1730867764.78 2024-11-06T04:36:05Z- AHRS_M2 msg: 1730867765.17 epoch sec; seqNo:107 2024-11-06T04:36:05Z- AHRS_M2 msg: 0.15 phi; -0.13 theta; 5.50 psi 2024-11-06T04:36:05Z- handleDepth()- Depth_Keller msg: 1730867765.17 epoch sec; seqNo:119; depth 6.64 2024-11-06T04:36:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867765.18 epoch sec; seqNo:119; 4061501.76 north; 594447.29 east; 36.69 lat; -121.94 lon; 6.64 depth; utmZone: 10 2024-11-06T04:36:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867763.96 2024-11-06T04:36:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:05Z- waiting for 1730867767.96, time is 1730867765.18 2024-11-06T04:36:05Z- AHRS_M2 msg: 1730867765.57 epoch sec; seqNo:108 2024-11-06T04:36:05Z- AHRS_M2 msg: 0.16 phi; -0.14 theta; 5.50 psi 2024-11-06T04:36:05Z- handleDepth()- Depth_Keller msg: 1730867765.58 epoch sec; seqNo:120; depth 6.71 2024-11-06T04:36:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867765.59 epoch sec; seqNo:120; 4061502.14 north; 594447.29 east; 36.69 lat; -121.94 lon; 6.71 depth; utmZone: 10 2024-11-06T04:36:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867763.96 2024-11-06T04:36:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:05Z- waiting for 1730867767.96, time is 1730867765.59 2024-11-06T04:36:05Z- AHRS_M2 msg: 1730867765.97 epoch sec; seqNo:109 2024-11-06T04:36:05Z- AHRS_M2 msg: 0.18 phi; -0.17 theta; 5.51 psi 2024-11-06T04:36:05Z- handleDepth()- Depth_Keller msg: 1730867765.97 epoch sec; seqNo:121; depth 6.78 2024-11-06T04:36:05Z- handleDVL() - DVL msg epochMillisec: 1730867765.98 2024-11-06T04:36:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:05Z- handleDvl() - RDI_Pathfinder msg: 1730867765.98 sec; seqNo:36; #beams:0 2024-11-06T04:36:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:05Z- handleDvl() - RDI_Pathfinder msg: velocities 1.07 , -0.23 , 0.57 2024-11-06T04:36:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867765.98 epoch sec; seqNo:121; 4061502.14 north; 594447.29 east; 36.69 lat; -121.94 lon; 6.78 depth; utmZone: 10 2024-11-06T04:36:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867763.96 2024-11-06T04:36:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:06Z- waiting for 1730867767.96, time is 1730867765.98 2024-11-06T04:36:06Z- AHRS_M2 msg: 1730867766.43 epoch sec; seqNo:110 2024-11-06T04:36:06Z- AHRS_M2 msg: 0.19 phi; -0.19 theta; 5.51 psi 2024-11-06T04:36:06Z- handleDepth()- Depth_Keller msg: 1730867766.44 epoch sec; seqNo:122; depth 6.81 2024-11-06T04:36:06Z- handleDVL() - DVL msg epochMillisec: 1730867766.50 2024-11-06T04:36:06Z- handleDvl() - RDI_Pathfinder msg: 1730867766.50 sec; seqNo:37; #beams:4 2024-11-06T04:36:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 96.92 , 75.24 , 78.51 , 93.26 2024-11-06T04:36:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.18 , 0.11 , 0.10 2024-11-06T04:36:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867766.53 epoch sec; seqNo:122; 4061502.14 north; 594447.29 east; 36.69 lat; -121.94 lon; 6.81 depth; utmZone: 10 2024-11-06T04:36:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867763.96 2024-11-06T04:36:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:06Z- waiting for 1730867767.96, time is 1730867766.53 2024-11-06T04:36:06Z- AHRS_M2 msg: 1730867766.78 epoch sec; seqNo:111 2024-11-06T04:36:06Z- AHRS_M2 msg: 0.19 phi; -0.20 theta; 5.50 psi 2024-11-06T04:36:06Z- handleDepth()- Depth_Keller msg: 1730867766.79 epoch sec; seqNo:123; depth 6.84 2024-11-06T04:36:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867766.80 epoch sec; seqNo:123; 4061502.14 north; 594447.29 east; 36.69 lat; -121.94 lon; 6.84 depth; utmZone: 10 2024-11-06T04:36:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867763.96 2024-11-06T04:36:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:06Z- waiting for 1730867767.96, time is 1730867766.80 2024-11-06T04:36:07Z- AHRS_M2 msg: 1730867767.18 epoch sec; seqNo:112 2024-11-06T04:36:07Z- AHRS_M2 msg: 0.18 phi; -0.21 theta; 5.50 psi 2024-11-06T04:36:07Z- handleDepth()- Depth_Keller msg: 1730867767.19 epoch sec; seqNo:124; depth 6.86 2024-11-06T04:36:07Z- handleDVL() - DVL msg epochMillisec: 1730867767.19 2024-11-06T04:36:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:07Z- handleDvl() - RDI_Pathfinder msg: 1730867767.19 sec; seqNo:38; #beams:0 2024-11-06T04:36:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.18 , 0.11 , 0.10 2024-11-06T04:36:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867767.20 epoch sec; seqNo:124; 4061502.52 north; 594447.29 east; 36.69 lat; -121.94 lon; 6.86 depth; utmZone: 10 2024-11-06T04:36:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867763.96 2024-11-06T04:36:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:07Z- waiting for 1730867767.96, time is 1730867767.20 2024-11-06T04:36:07Z- AHRS_M2 msg: 1730867767.59 epoch sec; seqNo:113 2024-11-06T04:36:07Z- AHRS_M2 msg: 0.16 phi; -0.20 theta; 5.50 psi 2024-11-06T04:36:07Z- handleDepth()- Depth_Keller msg: 1730867767.59 epoch sec; seqNo:125; depth 6.87 2024-11-06T04:36:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867767.62 epoch sec; seqNo:125; 4061502.52 north; 594447.29 east; 36.69 lat; -121.94 lon; 6.87 depth; utmZone: 10 2024-11-06T04:36:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867763.96 2024-11-06T04:36:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:07Z- waiting for 1730867767.96, time is 1730867767.62 2024-11-06T04:36:08Z- handleDepth()- Depth_Keller msg: 1730867768.01 epoch sec; seqNo:126; depth 6.90 2024-11-06T04:36:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867768.03 epoch sec; seqNo:126; 4061502.52 north; 594447.29 east; 36.69 lat; -121.94 lon; 6.90 depth; utmZone: 10 2024-11-06T04:36:08Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:36:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867763.96 2024-11-06T04:36:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:08Z- waiting for 1730867767.96, time is 1730867768.03 2024-11-06T04:36:08Z- LcmTrn::updateTrn() >>>> pose time 1730867768.03, meas time 1730867767.19 <<<< 2024-11-06T04:36:08Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867768.03 2024-11-06T04:36:08Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:36:08Z- TerrainNav::motionUpdate() - currEstimate = (4061503, 594447.29, 6.90) 2024-11-06T04:36:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:36:08Z- TerrainNav:: SubcloudNIS is 0.10, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:36:08Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:36:08Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:36:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:36:08Z- variances([0], [2], and psi_berg) = 313.200000 47.733925 0.000000 2024-11-06T04:36:08Z- publishEstimatesDecoder() - at 1730867768.03 sec 2024-11-06T04:36:08Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:36:08Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061530.92 594485.61 13.62 2024-11-06T04:36:08Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061539.68 594497.43 14.03 lat/lon: 36.69 -121.94 2024-11-06T04:36:08Z- LcmTrn::publishEstimatesDecoder() - COVAR: 313.20 47.73 2.03 0.00 2024-11-06T04:36:08Z- AHRS_M2 msg: 1730867768.44 epoch sec; seqNo:114 2024-11-06T04:36:08Z- AHRS_M2 msg: 0.10 phi; -0.14 theta; 5.50 psi 2024-11-06T04:36:08Z- handleDepth()- Depth_Keller msg: 1730867768.44 epoch sec; seqNo:127; depth 6.93 2024-11-06T04:36:08Z- handleDVL() - DVL msg epochMillisec: 1730867768.46 2024-11-06T04:36:08Z- handleDvl() - RDI_Pathfinder msg: 1730867768.46 sec; seqNo:39; #beams:4 2024-11-06T04:36:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.48 , 76.05 , 76.05 , 95.70 2024-11-06T04:36:08Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , -0.01 , -0.67 2024-11-06T04:36:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867768.51 epoch sec; seqNo:127; 4061502.89 north; 594446.06 east; 36.69 lat; -121.94 lon; 6.93 depth; utmZone: 10 2024-11-06T04:36:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867768.03 2024-11-06T04:36:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:08Z- waiting for 1730867772.03, time is 1730867768.51 2024-11-06T04:36:08Z- handleDepth()- Depth_Keller msg: 1730867768.82 epoch sec; seqNo:128; depth 6.96 2024-11-06T04:36:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867768.85 epoch sec; seqNo:128; 4061503.27 north; 594446.06 east; 36.69 lat; -121.94 lon; 6.96 depth; utmZone: 10 2024-11-06T04:36:08Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:36:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867768.03 2024-11-06T04:36:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:08Z- waiting for 1730867772.03, time is 1730867768.85 2024-11-06T04:36:09Z- AHRS_M2 msg: 1730867769.22 epoch sec; seqNo:115 2024-11-06T04:36:09Z- AHRS_M2 msg: 0.10 phi; -0.13 theta; 5.49 psi 2024-11-06T04:36:09Z- handleDepth()- Depth_Keller msg: 1730867769.23 epoch sec; seqNo:129; depth 7.00 2024-11-06T04:36:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867769.24 epoch sec; seqNo:129; 4061503.64 north; 594446.05 east; 36.69 lat; -121.94 lon; 7.00 depth; utmZone: 10 2024-11-06T04:36:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867768.03 2024-11-06T04:36:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:09Z- waiting for 1730867772.03, time is 1730867769.24 2024-11-06T04:36:09Z- AHRS_M2 msg: 1730867769.61 epoch sec; seqNo:116 2024-11-06T04:36:09Z- AHRS_M2 msg: 0.11 phi; -0.13 theta; 5.49 psi 2024-11-06T04:36:09Z- handleDepth()- Depth_Keller msg: 1730867769.62 epoch sec; seqNo:130; depth 7.08 2024-11-06T04:36:09Z- handleDVL() - DVL msg epochMillisec: 1730867769.62 2024-11-06T04:36:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:09Z- handleDvl() - RDI_Pathfinder msg: 1730867769.62 sec; seqNo:40; #beams:0 2024-11-06T04:36:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:09Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , -0.01 , -0.67 2024-11-06T04:36:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867769.64 epoch sec; seqNo:130; 4061504.02 north; 594446.05 east; 36.69 lat; -121.94 lon; 7.08 depth; utmZone: 10 2024-11-06T04:36:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867768.03 2024-11-06T04:36:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:09Z- waiting for 1730867772.03, time is 1730867769.64 2024-11-06T04:36:10Z- AHRS_M2 msg: 1730867770.01 epoch sec; seqNo:117 2024-11-06T04:36:10Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.49 psi 2024-11-06T04:36:10Z- handleDepth()- Depth_Keller msg: 1730867770.02 epoch sec; seqNo:131; depth 7.12 2024-11-06T04:36:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867770.03 epoch sec; seqNo:131; 4061504.39 north; 594444.82 east; 36.69 lat; -121.94 lon; 7.12 depth; utmZone: 10 2024-11-06T04:36:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867768.03 2024-11-06T04:36:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:10Z- waiting for 1730867772.03, time is 1730867770.03 2024-11-06T04:36:10Z- handleDepth()- Depth_Keller msg: 1730867770.46 epoch sec; seqNo:132; depth 7.18 2024-11-06T04:36:10Z- handleDVL() - DVL msg epochMillisec: 1730867770.48 2024-11-06T04:36:10Z- handleDvl() - RDI_Pathfinder msg: 1730867770.48 sec; seqNo:41; #beams:4 2024-11-06T04:36:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.83 , 74.42 , 76.46 , 90.83 2024-11-06T04:36:10Z- handleDvl() - RDI_Pathfinder msg: velocities 1.25 , -0.11 , -0.00 2024-11-06T04:36:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867770.50 epoch sec; seqNo:132; 4061504.77 north; 594444.82 east; 36.69 lat; -121.94 lon; 7.18 depth; utmZone: 10 2024-11-06T04:36:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867768.03 2024-11-06T04:36:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:10Z- waiting for 1730867772.03, time is 1730867770.50 2024-11-06T04:36:10Z- AHRS_M2 msg: 1730867770.82 epoch sec; seqNo:118 2024-11-06T04:36:10Z- AHRS_M2 msg: 0.14 phi; -0.14 theta; 5.48 psi 2024-11-06T04:36:10Z- handleDepth()- Depth_Keller msg: 1730867770.84 epoch sec; seqNo:133; depth 7.23 2024-11-06T04:36:10Z- handleDVL() - DVL msg epochMillisec: 1730867770.85 2024-11-06T04:36:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:10Z- handleDvl() - RDI_Pathfinder msg: 1730867770.85 sec; seqNo:42; #beams:0 2024-11-06T04:36:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:10Z- handleDvl() - RDI_Pathfinder msg: velocities 1.25 , -0.11 , -0.00 2024-11-06T04:36:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867770.86 epoch sec; seqNo:133; 4061504.77 north; 594444.82 east; 36.69 lat; -121.94 lon; 7.23 depth; utmZone: 10 2024-11-06T04:36:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867768.03 2024-11-06T04:36:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:10Z- waiting for 1730867772.03, time is 1730867770.86 2024-11-06T04:36:11Z- AHRS_M2 msg: 1730867771.23 epoch sec; seqNo:119 2024-11-06T04:36:11Z- AHRS_M2 msg: 0.15 phi; -0.13 theta; 5.48 psi 2024-11-06T04:36:11Z- handleDepth()- Depth_Keller msg: 1730867771.24 epoch sec; seqNo:134; depth 7.27 2024-11-06T04:36:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867771.26 epoch sec; seqNo:134; 4061505.13 north; 594443.60 east; 36.69 lat; -121.94 lon; 7.27 depth; utmZone: 10 2024-11-06T04:36:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867768.03 2024-11-06T04:36:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:11Z- waiting for 1730867772.03, time is 1730867771.26 2024-11-06T04:36:11Z- AHRS_M2 msg: 1730867771.63 epoch sec; seqNo:120 2024-11-06T04:36:11Z- AHRS_M2 msg: 0.15 phi; -0.13 theta; 5.48 psi 2024-11-06T04:36:11Z- handleDepth()- Depth_Keller msg: 1730867771.64 epoch sec; seqNo:135; depth 7.32 2024-11-06T04:36:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867771.65 epoch sec; seqNo:135; 4061505.51 north; 594443.59 east; 36.69 lat; -121.94 lon; 7.32 depth; utmZone: 10 2024-11-06T04:36:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867768.03 2024-11-06T04:36:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:11Z- waiting for 1730867772.03, time is 1730867771.65 2024-11-06T04:36:12Z- AHRS_M2 msg: 1730867772.03 epoch sec; seqNo:121 2024-11-06T04:36:12Z- AHRS_M2 msg: 0.14 phi; -0.13 theta; 5.48 psi 2024-11-06T04:36:12Z- handleDepth()- Depth_Keller msg: 1730867772.04 epoch sec; seqNo:136; depth 7.37 2024-11-06T04:36:12Z- handleDVL() - DVL msg epochMillisec: 1730867772.04 2024-11-06T04:36:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:12Z- handleDvl() - RDI_Pathfinder msg: 1730867772.04 sec; seqNo:43; #beams:0 2024-11-06T04:36:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:12Z- handleDvl() - RDI_Pathfinder msg: velocities 1.25 , -0.11 , -0.00 2024-11-06T04:36:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867772.05 epoch sec; seqNo:136; 4061505.89 north; 594443.59 east; 36.69 lat; -121.94 lon; 7.37 depth; utmZone: 10 2024-11-06T04:36:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867768.03 2024-11-06T04:36:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:12Z- waiting for 1730867772.03, time is 1730867772.05 2024-11-06T04:36:12Z- LcmTrn::updateTrn() >>>> pose time 1730867772.05, meas time 1730867772.04 <<<< 2024-11-06T04:36:12Z- LcmTrn::updateTrn() equating pose 1730867772.04 and meas times 1730867772.04 2024-11-06T04:36:12Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867772.04 2024-11-06T04:36:12Z- TerrainNav::motionUpdate() - currEstimate = (4061506, 594443.59, 7.37) 2024-11-06T04:36:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:36:12Z- TerrainNav:: SubcloudNIS is 0.10, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:36:12Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:36:12Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:36:12Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:36:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:36:12Z- variances([0], [2], and psi_berg) = 313.222464 47.744730 0.000000 2024-11-06T04:36:12Z- publishEstimatesDecoder() - at 1730867772.04 sec 2024-11-06T04:36:12Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:36:12Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061534.37 594481.87 13.85 2024-11-06T04:36:12Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061543.05 594493.74 14.50 lat/lon: 36.69 -121.94 2024-11-06T04:36:12Z- LcmTrn::publishEstimatesDecoder() - COVAR: 313.22 47.74 2.28 0.00 2024-11-06T04:36:12Z- AHRS_M2 msg: 1730867772.43 epoch sec; seqNo:122 2024-11-06T04:36:12Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.47 psi 2024-11-06T04:36:12Z- handleDepth()- Depth_Keller msg: 1730867772.44 epoch sec; seqNo:137; depth 7.39 2024-11-06T04:36:12Z- handleDVL() - DVL msg epochMillisec: 1730867772.47 2024-11-06T04:36:12Z- handleDvl() - RDI_Pathfinder msg: 1730867772.47 sec; seqNo:44; #beams:4 2024-11-06T04:36:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.53 , 73.60 , 77.28 , 88.66 2024-11-06T04:36:12Z- handleDvl() - RDI_Pathfinder msg: velocities 1.16 , -0.15 , 0.12 2024-11-06T04:36:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867772.49 epoch sec; seqNo:137; 4061506.26 north; 594442.36 east; 36.69 lat; -121.94 lon; 7.39 depth; utmZone: 10 2024-11-06T04:36:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867772.04 2024-11-06T04:36:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:12Z- waiting for 1730867776.04, time is 1730867772.49 2024-11-06T04:36:12Z- AHRS_M2 msg: 1730867772.85 epoch sec; seqNo:123 2024-11-06T04:36:12Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.47 psi 2024-11-06T04:36:12Z- handleDepth()- Depth_Keller msg: 1730867772.85 epoch sec; seqNo:138; depth 7.46 2024-11-06T04:36:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867772.87 epoch sec; seqNo:138; 4061506.26 north; 594442.36 east; 36.69 lat; -121.94 lon; 7.46 depth; utmZone: 10 2024-11-06T04:36:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867772.04 2024-11-06T04:36:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:13Z- waiting for 1730867776.04, time is 1730867772.87 2024-11-06T04:36:13Z- AHRS_M2 msg: 1730867773.24 epoch sec; seqNo:124 2024-11-06T04:36:13Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.47 psi 2024-11-06T04:36:13Z- handleDepth()- Depth_Keller msg: 1730867773.25 epoch sec; seqNo:139; depth 7.51 2024-11-06T04:36:13Z- handleDVL() - DVL msg epochMillisec: 1730867773.25 2024-11-06T04:36:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:13Z- handleDvl() - RDI_Pathfinder msg: 1730867773.25 sec; seqNo:45; #beams:0 2024-11-06T04:36:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:13Z- handleDvl() - RDI_Pathfinder msg: velocities 1.16 , -0.15 , 0.12 2024-11-06T04:36:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867773.27 epoch sec; seqNo:139; 4061506.63 north; 594442.36 east; 36.69 lat; -121.94 lon; 7.51 depth; utmZone: 10 2024-11-06T04:36:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867772.04 2024-11-06T04:36:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:13Z- waiting for 1730867776.04, time is 1730867773.27 2024-11-06T04:36:13Z- AHRS_M2 msg: 1730867773.65 epoch sec; seqNo:125 2024-11-06T04:36:13Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.47 psi 2024-11-06T04:36:13Z- handleDepth()- Depth_Keller msg: 1730867773.65 epoch sec; seqNo:140; depth 7.54 2024-11-06T04:36:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867773.67 epoch sec; seqNo:140; 4061507.01 north; 594442.35 east; 36.69 lat; -121.94 lon; 7.54 depth; utmZone: 10 2024-11-06T04:36:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867772.04 2024-11-06T04:36:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:13Z- waiting for 1730867776.04, time is 1730867773.67 2024-11-06T04:36:14Z- AHRS_M2 msg: 1730867774.05 epoch sec; seqNo:126 2024-11-06T04:36:14Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.47 psi 2024-11-06T04:36:14Z- handleDepth()- Depth_Keller msg: 1730867774.06 epoch sec; seqNo:141; depth 7.59 2024-11-06T04:36:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867774.07 epoch sec; seqNo:141; 4061507.00 north; 594441.13 east; 36.69 lat; -121.94 lon; 7.59 depth; utmZone: 10 2024-11-06T04:36:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867772.04 2024-11-06T04:36:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:14Z- waiting for 1730867776.04, time is 1730867774.07 2024-11-06T04:36:14Z- AHRS_M2 msg: 1730867774.45 epoch sec; seqNo:127 2024-11-06T04:36:14Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.47 psi 2024-11-06T04:36:14Z- handleDepth()- Depth_Keller msg: 1730867774.46 epoch sec; seqNo:142; depth 7.67 2024-11-06T04:36:14Z- handleDVL() - DVL msg epochMillisec: 1730867774.48 2024-11-06T04:36:14Z- handleDvl() - RDI_Pathfinder msg: 1730867774.48 sec; seqNo:46; #beams:4 2024-11-06T04:36:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.25 , 74.42 , 77.69 , 88.59 2024-11-06T04:36:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.23 , 0.40 2024-11-06T04:36:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867774.52 epoch sec; seqNo:142; 4061507.38 north; 594441.13 east; 36.69 lat; -121.94 lon; 7.67 depth; utmZone: 10 2024-11-06T04:36:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867772.04 2024-11-06T04:36:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:14Z- waiting for 1730867776.04, time is 1730867774.52 2024-11-06T04:36:14Z- AHRS_M2 msg: 1730867774.87 epoch sec; seqNo:128 2024-11-06T04:36:14Z- AHRS_M2 msg: 0.14 phi; -0.13 theta; 5.47 psi 2024-11-06T04:36:14Z- handleDepth()- Depth_Keller msg: 1730867774.88 epoch sec; seqNo:143; depth 7.71 2024-11-06T04:36:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867774.90 epoch sec; seqNo:143; 4061507.38 north; 594441.13 east; 36.69 lat; -121.94 lon; 7.71 depth; utmZone: 10 2024-11-06T04:36:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867772.04 2024-11-06T04:36:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:15Z- waiting for 1730867776.04, time is 1730867774.90 2024-11-06T04:36:15Z- AHRS_M2 msg: 1730867775.27 epoch sec; seqNo:129 2024-11-06T04:36:15Z- AHRS_M2 msg: 0.15 phi; -0.14 theta; 5.48 psi 2024-11-06T04:36:15Z- handleDepth()- Depth_Keller msg: 1730867775.27 epoch sec; seqNo:144; depth 7.75 2024-11-06T04:36:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867775.28 epoch sec; seqNo:144; 4061507.74 north; 594439.91 east; 36.69 lat; -121.94 lon; 7.75 depth; utmZone: 10 2024-11-06T04:36:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867772.04 2024-11-06T04:36:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:15Z- waiting for 1730867776.04, time is 1730867775.28 2024-11-06T04:36:15Z- AHRS_M2 msg: 1730867775.67 epoch sec; seqNo:130 2024-11-06T04:36:15Z- AHRS_M2 msg: 0.15 phi; -0.15 theta; 5.48 psi 2024-11-06T04:36:15Z- handleDepth()- Depth_Keller msg: 1730867775.67 epoch sec; seqNo:145; depth 7.81 2024-11-06T04:36:15Z- handleDVL() - DVL msg epochMillisec: 1730867775.68 2024-11-06T04:36:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:15Z- handleDvl() - RDI_Pathfinder msg: 1730867775.68 sec; seqNo:47; #beams:0 2024-11-06T04:36:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.23 , 0.40 2024-11-06T04:36:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867775.68 epoch sec; seqNo:145; 4061507.74 north; 594439.91 east; 36.69 lat; -121.94 lon; 7.81 depth; utmZone: 10 2024-11-06T04:36:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867772.04 2024-11-06T04:36:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:15Z- waiting for 1730867776.04, time is 1730867775.68 2024-11-06T04:36:16Z- AHRS_M2 msg: 1730867776.07 epoch sec; seqNo:131 2024-11-06T04:36:16Z- AHRS_M2 msg: 0.15 phi; -0.16 theta; 5.48 psi 2024-11-06T04:36:16Z- handleDepth()- Depth_Keller msg: 1730867776.08 epoch sec; seqNo:146; depth 7.84 2024-11-06T04:36:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867776.09 epoch sec; seqNo:146; 4061507.74 north; 594439.91 east; 36.69 lat; -121.94 lon; 7.84 depth; utmZone: 10 2024-11-06T04:36:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867772.04 2024-11-06T04:36:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:16Z- waiting for 1730867776.04, time is 1730867776.09 2024-11-06T04:36:16Z- LcmTrn::updateTrn() >>>> pose time 1730867776.09, meas time 1730867775.68 <<<< 2024-11-06T04:36:16Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867776.09 2024-11-06T04:36:16Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:36:16Z- TerrainNav::motionUpdate() - currEstimate = (4061508, 594439.91, 7.84) 2024-11-06T04:36:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:36:16Z- TerrainNav:: SubcloudNIS is 0.10, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:36:16Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:36:16Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:36:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:36:16Z- variances([0], [2], and psi_berg) = 313.262899 47.738634 0.000000 2024-11-06T04:36:16Z- publishEstimatesDecoder() - at 1730867776.09 sec 2024-11-06T04:36:16Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:36:16Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061536.24 594478.19 14.11 2024-11-06T04:36:16Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061544.90 594490.05 14.98 lat/lon: 36.69 -121.94 2024-11-06T04:36:16Z- LcmTrn::publishEstimatesDecoder() - COVAR: 313.26 47.74 2.53 0.00 2024-11-06T04:36:16Z- AHRS_M2 msg: 1730867776.48 epoch sec; seqNo:132 2024-11-06T04:36:16Z- AHRS_M2 msg: 0.15 phi; -0.16 theta; 5.49 psi 2024-11-06T04:36:16Z- handleDepth()- Depth_Keller msg: 1730867776.49 epoch sec; seqNo:147; depth 7.88 2024-11-06T04:36:16Z- handleDVL() - DVL msg epochMillisec: 1730867776.52 2024-11-06T04:36:16Z- handleDvl() - RDI_Pathfinder msg: 1730867776.52 sec; seqNo:48; #beams:4 2024-11-06T04:36:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.68 , 74.42 , 78.10 , 89.81 2024-11-06T04:36:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.51 , -0.08 , -0.01 2024-11-06T04:36:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867776.56 epoch sec; seqNo:147; 4061508.12 north; 594439.90 east; 36.69 lat; -121.94 lon; 7.88 depth; utmZone: 10 2024-11-06T04:36:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867776.09 2024-11-06T04:36:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:16Z- waiting for 1730867780.09, time is 1730867776.56 2024-11-06T04:36:16Z- handleDepth()- Depth_Keller msg: 1730867776.90 epoch sec; seqNo:148; depth 7.92 2024-11-06T04:36:16Z- handleDVL() - DVL msg epochMillisec: 1730867776.91 2024-11-06T04:36:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:16Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:16Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:16Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:16Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:16Z- handleDvl() - RDI_Pathfinder msg: 1730867776.91 sec; seqNo:49; #beams:0 2024-11-06T04:36:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.51 , -0.08 , -0.01 2024-11-06T04:36:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867776.92 epoch sec; seqNo:148; 4061508.12 north; 594439.90 east; 36.69 lat; -121.94 lon; 7.92 depth; utmZone: 10 2024-11-06T04:36:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867776.09 2024-11-06T04:36:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:17Z- waiting for 1730867780.09, time is 1730867776.92 2024-11-06T04:36:17Z- AHRS_M2 msg: 1730867777.28 epoch sec; seqNo:133 2024-11-06T04:36:17Z- AHRS_M2 msg: 0.15 phi; -0.16 theta; 5.50 psi 2024-11-06T04:36:17Z- handleDepth()- Depth_Keller msg: 1730867777.29 epoch sec; seqNo:149; depth 7.95 2024-11-06T04:36:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867777.30 epoch sec; seqNo:149; 4061508.11 north; 594438.68 east; 36.69 lat; -121.94 lon; 7.95 depth; utmZone: 10 2024-11-06T04:36:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867776.09 2024-11-06T04:36:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:17Z- waiting for 1730867780.09, time is 1730867777.30 2024-11-06T04:36:17Z- AHRS_M2 msg: 1730867777.69 epoch sec; seqNo:134 2024-11-06T04:36:17Z- AHRS_M2 msg: 0.14 phi; -0.16 theta; 5.51 psi 2024-11-06T04:36:17Z- handleDepth()- Depth_Keller msg: 1730867777.69 epoch sec; seqNo:150; depth 7.99 2024-11-06T04:36:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867777.70 epoch sec; seqNo:150; 4061508.49 north; 594438.68 east; 36.69 lat; -121.94 lon; 7.99 depth; utmZone: 10 2024-11-06T04:36:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867776.09 2024-11-06T04:36:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:17Z- waiting for 1730867780.09, time is 1730867777.70 2024-11-06T04:36:18Z- AHRS_M2 msg: 1730867778.09 epoch sec; seqNo:135 2024-11-06T04:36:18Z- AHRS_M2 msg: 0.14 phi; -0.15 theta; 5.52 psi 2024-11-06T04:36:18Z- handleDepth()- Depth_Keller msg: 1730867778.10 epoch sec; seqNo:151; depth 8.02 2024-11-06T04:36:18Z- handleDVL() - DVL msg epochMillisec: 1730867778.11 2024-11-06T04:36:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:18Z- handleDvl() - RDI_Pathfinder msg: 1730867778.11 sec; seqNo:50; #beams:0 2024-11-06T04:36:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.51 , -0.08 , -0.01 2024-11-06T04:36:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867778.12 epoch sec; seqNo:151; 4061508.49 north; 594438.68 east; 36.69 lat; -121.94 lon; 8.02 depth; utmZone: 10 2024-11-06T04:36:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867776.09 2024-11-06T04:36:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:18Z- waiting for 1730867780.09, time is 1730867778.12 2024-11-06T04:36:18Z- handleDepth()- Depth_Keller msg: 1730867778.52 epoch sec; seqNo:152; depth 8.05 2024-11-06T04:36:18Z- handleDVL() - DVL msg epochMillisec: 1730867778.57 2024-11-06T04:36:18Z- handleDvl() - RDI_Pathfinder msg: 1730867778.57 sec; seqNo:51; #beams:4 2024-11-06T04:36:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.18 , 74.01 , 76.46 , 92.18 2024-11-06T04:36:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.34 2024-11-06T04:36:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867778.59 epoch sec; seqNo:152; 4061508.87 north; 594438.67 east; 36.69 lat; -121.94 lon; 8.05 depth; utmZone: 10 2024-11-06T04:36:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867776.09 2024-11-06T04:36:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:18Z- waiting for 1730867780.09, time is 1730867778.59 2024-11-06T04:36:18Z- handleDepth()- Depth_Keller msg: 1730867778.92 epoch sec; seqNo:153; depth 8.11 2024-11-06T04:36:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867778.94 epoch sec; seqNo:153; 4061508.87 north; 594438.67 east; 36.69 lat; -121.94 lon; 8.11 depth; utmZone: 10 2024-11-06T04:36:19Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:36:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867776.09 2024-11-06T04:36:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:19Z- waiting for 1730867780.09, time is 1730867778.94 2024-11-06T04:36:19Z- AHRS_M2 msg: 1730867779.31 epoch sec; seqNo:136 2024-11-06T04:36:19Z- AHRS_M2 msg: 0.15 phi; -0.14 theta; 5.52 psi 2024-11-06T04:36:19Z- handleDepth()- Depth_Keller msg: 1730867779.31 epoch sec; seqNo:154; depth 8.14 2024-11-06T04:36:19Z- handleDVL() - DVL msg epochMillisec: 1730867779.32 2024-11-06T04:36:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:19Z- handleDvl() - RDI_Pathfinder msg: 1730867779.32 sec; seqNo:52; #beams:0 2024-11-06T04:36:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.34 2024-11-06T04:36:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867779.33 epoch sec; seqNo:154; 4061509.25 north; 594438.67 east; 36.69 lat; -121.94 lon; 8.14 depth; utmZone: 10 2024-11-06T04:36:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867776.09 2024-11-06T04:36:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:19Z- waiting for 1730867780.09, time is 1730867779.33 2024-11-06T04:36:19Z- AHRS_M2 msg: 1730867779.71 epoch sec; seqNo:137 2024-11-06T04:36:19Z- AHRS_M2 msg: 0.14 phi; -0.13 theta; 5.51 psi 2024-11-06T04:36:19Z- handleDepth()- Depth_Keller msg: 1730867779.72 epoch sec; seqNo:155; depth 8.19 2024-11-06T04:36:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867779.73 epoch sec; seqNo:155; 4061509.61 north; 594437.44 east; 36.69 lat; -121.94 lon; 8.19 depth; utmZone: 10 2024-11-06T04:36:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867776.09 2024-11-06T04:36:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:19Z- waiting for 1730867780.09, time is 1730867779.73 2024-11-06T04:36:20Z- AHRS_M2 msg: 1730867780.12 epoch sec; seqNo:138 2024-11-06T04:36:20Z- AHRS_M2 msg: 0.14 phi; -0.13 theta; 5.51 psi 2024-11-06T04:36:20Z- handleDepth()- Depth_Keller msg: 1730867780.12 epoch sec; seqNo:156; depth 8.24 2024-11-06T04:36:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867780.13 epoch sec; seqNo:156; 4061509.99 north; 594437.44 east; 36.69 lat; -121.94 lon; 8.24 depth; utmZone: 10 2024-11-06T04:36:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867776.09 2024-11-06T04:36:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:20Z- waiting for 1730867780.09, time is 1730867780.13 2024-11-06T04:36:20Z- LcmTrn::updateTrn() >>>> pose time 1730867780.13, meas time 1730867779.32 <<<< 2024-11-06T04:36:20Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867780.13 2024-11-06T04:36:20Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:36:20Z- TerrainNav::motionUpdate() - currEstimate = (4061510, 594437.44, 8.24) 2024-11-06T04:36:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:36:20Z- TerrainNav:: SubcloudNIS is 0.10, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:36:20Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:36:20Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:36:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:36:20Z- variances([0], [2], and psi_berg) = 313.259467 47.747459 0.000000 2024-11-06T04:36:20Z- publishEstimatesDecoder() - at 1730867780.13 sec 2024-11-06T04:36:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:36:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061538.44 594475.72 14.46 2024-11-06T04:36:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061547.15 594487.59 15.38 lat/lon: 36.69 -121.94 2024-11-06T04:36:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 313.26 47.75 2.74 0.00 2024-11-06T04:36:20Z- AHRS_M2 msg: 1730867780.53 epoch sec; seqNo:139 2024-11-06T04:36:20Z- AHRS_M2 msg: 0.14 phi; -0.13 theta; 5.50 psi 2024-11-06T04:36:20Z- handleDepth()- Depth_Keller msg: 1730867780.54 epoch sec; seqNo:157; depth 8.28 2024-11-06T04:36:20Z- handleDVL() - DVL msg epochMillisec: 1730867780.56 2024-11-06T04:36:20Z- handleDvl() - RDI_Pathfinder msg: 1730867780.56 sec; seqNo:53; #beams:4 2024-11-06T04:36:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.68 , 73.19 , 76.46 , 89.81 2024-11-06T04:36:20Z- handleDvl() - RDI_Pathfinder msg: velocities 1.24 , -0.12 , -0.00 2024-11-06T04:36:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867780.61 epoch sec; seqNo:157; 4061510.37 north; 594437.44 east; 36.69 lat; -121.94 lon; 8.28 depth; utmZone: 10 2024-11-06T04:36:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867780.13 2024-11-06T04:36:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:20Z- waiting for 1730867784.13, time is 1730867780.61 2024-11-06T04:36:20Z- AHRS_M2 msg: 1730867780.92 epoch sec; seqNo:140 2024-11-06T04:36:20Z- AHRS_M2 msg: 0.14 phi; -0.13 theta; 5.50 psi 2024-11-06T04:36:20Z- handleDepth()- Depth_Keller msg: 1730867780.93 epoch sec; seqNo:158; depth 8.33 2024-11-06T04:36:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867780.95 epoch sec; seqNo:158; 4061510.36 north; 594436.22 east; 36.69 lat; -121.94 lon; 8.33 depth; utmZone: 10 2024-11-06T04:36:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867780.13 2024-11-06T04:36:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:21Z- waiting for 1730867784.13, time is 1730867780.95 2024-11-06T04:36:21Z- AHRS_M2 msg: 1730867781.32 epoch sec; seqNo:141 2024-11-06T04:36:21Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.50 psi 2024-11-06T04:36:21Z- handleDepth()- Depth_Keller msg: 1730867781.33 epoch sec; seqNo:159; depth 8.38 2024-11-06T04:36:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867781.34 epoch sec; seqNo:159; 4061510.73 north; 594436.21 east; 36.69 lat; -121.94 lon; 8.38 depth; utmZone: 10 2024-11-06T04:36:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867780.13 2024-11-06T04:36:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:21Z- waiting for 1730867784.13, time is 1730867781.34 2024-11-06T04:36:21Z- AHRS_M2 msg: 1730867781.73 epoch sec; seqNo:142 2024-11-06T04:36:21Z- AHRS_M2 msg: 0.13 phi; -0.14 theta; 5.49 psi 2024-11-06T04:36:21Z- handleDepth()- Depth_Keller msg: 1730867781.73 epoch sec; seqNo:160; depth 8.41 2024-11-06T04:36:21Z- handleDVL() - DVL msg epochMillisec: 1730867781.74 2024-11-06T04:36:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:21Z- handleDvl() - RDI_Pathfinder msg: 1730867781.74 sec; seqNo:54; #beams:0 2024-11-06T04:36:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:21Z- handleDvl() - RDI_Pathfinder msg: velocities 1.24 , -0.12 , -0.00 2024-11-06T04:36:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867781.75 epoch sec; seqNo:160; 4061511.11 north; 594436.21 east; 36.69 lat; -121.94 lon; 8.41 depth; utmZone: 10 2024-11-06T04:36:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867780.13 2024-11-06T04:36:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:21Z- waiting for 1730867784.13, time is 1730867781.75 2024-11-06T04:36:22Z- AHRS_M2 msg: 1730867782.13 epoch sec; seqNo:143 2024-11-06T04:36:22Z- AHRS_M2 msg: 0.13 phi; -0.14 theta; 5.48 psi 2024-11-06T04:36:22Z- handleDepth()- Depth_Keller msg: 1730867782.14 epoch sec; seqNo:161; depth 8.47 2024-11-06T04:36:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867782.15 epoch sec; seqNo:161; 4061511.48 north; 594434.98 east; 36.69 lat; -121.94 lon; 8.47 depth; utmZone: 10 2024-11-06T04:36:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867780.13 2024-11-06T04:36:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:22Z- waiting for 1730867784.13, time is 1730867782.15 2024-11-06T04:36:22Z- AHRS_M2 msg: 1730867782.54 epoch sec; seqNo:144 2024-11-06T04:36:22Z- AHRS_M2 msg: 0.13 phi; -0.15 theta; 5.48 psi 2024-11-06T04:36:22Z- handleDepth()- Depth_Keller msg: 1730867782.55 epoch sec; seqNo:162; depth 8.52 2024-11-06T04:36:22Z- handleDVL() - DVL msg epochMillisec: 1730867782.58 2024-11-06T04:36:22Z- handleDvl() - RDI_Pathfinder msg: 1730867782.58 sec; seqNo:55; #beams:4 2024-11-06T04:36:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 98.34 , 73.19 , 76.46 , 89.81 2024-11-06T04:36:22Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.05 , 0.13 2024-11-06T04:36:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867782.59 epoch sec; seqNo:162; 4061511.48 north; 594434.98 east; 36.69 lat; -121.94 lon; 8.52 depth; utmZone: 10 2024-11-06T04:36:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867780.13 2024-11-06T04:36:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:22Z- waiting for 1730867784.13, time is 1730867782.59 2024-11-06T04:36:22Z- AHRS_M2 msg: 1730867782.94 epoch sec; seqNo:145 2024-11-06T04:36:22Z- AHRS_M2 msg: 0.13 phi; -0.16 theta; 5.48 psi 2024-11-06T04:36:22Z- handleDepth()- Depth_Keller msg: 1730867782.95 epoch sec; seqNo:163; depth 8.56 2024-11-06T04:36:22Z- handleDVL() - DVL msg epochMillisec: 1730867782.95 2024-11-06T04:36:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:22Z- handleDvl() - RDI_Pathfinder msg: 1730867782.95 sec; seqNo:56; #beams:0 2024-11-06T04:36:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:22Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.05 , 0.13 2024-11-06T04:36:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867783.00 epoch sec; seqNo:163; 4061511.86 north; 594434.98 east; 36.69 lat; -121.94 lon; 8.56 depth; utmZone: 10 2024-11-06T04:36:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867780.13 2024-11-06T04:36:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:23Z- waiting for 1730867784.13, time is 1730867783.00 2024-11-06T04:36:23Z- AHRS_M2 msg: 1730867783.35 epoch sec; seqNo:146 2024-11-06T04:36:23Z- AHRS_M2 msg: 0.12 phi; -0.16 theta; 5.48 psi 2024-11-06T04:36:23Z- handleDepth()- Depth_Keller msg: 1730867783.35 epoch sec; seqNo:164; depth 8.60 2024-11-06T04:36:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867783.36 epoch sec; seqNo:164; 4061512.24 north; 594434.97 east; 36.69 lat; -121.94 lon; 8.60 depth; utmZone: 10 2024-11-06T04:36:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867780.13 2024-11-06T04:36:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:23Z- waiting for 1730867784.13, time is 1730867783.36 2024-11-06T04:36:23Z- AHRS_M2 msg: 1730867783.75 epoch sec; seqNo:147 2024-11-06T04:36:23Z- AHRS_M2 msg: 0.10 phi; -0.16 theta; 5.48 psi 2024-11-06T04:36:23Z- handleDepth()- Depth_Keller msg: 1730867783.76 epoch sec; seqNo:165; depth 8.63 2024-11-06T04:36:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867783.78 epoch sec; seqNo:165; 4061512.62 north; 594434.97 east; 36.69 lat; -121.94 lon; 8.63 depth; utmZone: 10 2024-11-06T04:36:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867780.13 2024-11-06T04:36:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:23Z- waiting for 1730867784.13, time is 1730867783.78 2024-11-06T04:36:24Z- AHRS_M2 msg: 1730867784.15 epoch sec; seqNo:148 2024-11-06T04:36:24Z- AHRS_M2 msg: 0.10 phi; -0.16 theta; 5.48 psi 2024-11-06T04:36:24Z- handleDepth()- Depth_Keller msg: 1730867784.16 epoch sec; seqNo:166; depth 8.68 2024-11-06T04:36:24Z- handleDVL() - DVL msg epochMillisec: 1730867784.16 2024-11-06T04:36:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:24Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:24Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:24Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:24Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:24Z- handleDvl() - RDI_Pathfinder msg: 1730867784.16 sec; seqNo:57; #beams:0 2024-11-06T04:36:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , 0.05 , 0.13 2024-11-06T04:36:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867784.18 epoch sec; seqNo:166; 4061512.60 north; 594433.75 east; 36.69 lat; -121.94 lon; 8.68 depth; utmZone: 10 2024-11-06T04:36:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867780.13 2024-11-06T04:36:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:24Z- waiting for 1730867784.13, time is 1730867784.18 2024-11-06T04:36:24Z- LcmTrn::updateTrn() >>>> pose time 1730867784.18, meas time 1730867784.16 <<<< 2024-11-06T04:36:24Z- LcmTrn::updateTrn() equating pose 1730867784.16 and meas times 1730867784.16 2024-11-06T04:36:24Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867784.16 2024-11-06T04:36:24Z- TerrainNav::motionUpdate() - currEstimate = (4061513, 594433.75, 8.68) 2024-11-06T04:36:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:36:24Z- TerrainNav:: SubcloudNIS is 0.10, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:36:24Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:36:24Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:36:24Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:36:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:36:24Z- variances([0], [2], and psi_berg) = 313.246533 47.743585 0.000000 2024-11-06T04:36:24Z- publishEstimatesDecoder() - at 1730867784.16 sec 2024-11-06T04:36:24Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:36:24Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061541.05 594471.96 15.31 2024-11-06T04:36:24Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061549.76 594483.90 15.81 lat/lon: 36.69 -121.94 2024-11-06T04:36:24Z- LcmTrn::publishEstimatesDecoder() - COVAR: 313.25 47.74 2.96 0.00 2024-11-06T04:36:24Z- AHRS_M2 msg: 1730867784.56 epoch sec; seqNo:149 2024-11-06T04:36:24Z- AHRS_M2 msg: 0.09 phi; -0.16 theta; 5.47 psi 2024-11-06T04:36:24Z- handleDepth()- Depth_Keller msg: 1730867784.58 epoch sec; seqNo:167; depth 8.72 2024-11-06T04:36:24Z- handleDVL() - DVL msg epochMillisec: 1730867784.61 2024-11-06T04:36:24Z- handleDvl() - RDI_Pathfinder msg: 1730867784.61 sec; seqNo:58; #beams:4 2024-11-06T04:36:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.74 , 74.83 , 74.83 , 90.96 2024-11-06T04:36:24Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , -0.12 , -0.03 2024-11-06T04:36:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867784.62 epoch sec; seqNo:167; 4061512.98 north; 594433.75 east; 36.69 lat; -121.94 lon; 8.72 depth; utmZone: 10 2024-11-06T04:36:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867784.16 2024-11-06T04:36:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:24Z- waiting for 1730867788.16, time is 1730867784.62 2024-11-06T04:36:24Z- AHRS_M2 msg: 1730867784.96 epoch sec; seqNo:150 2024-11-06T04:36:24Z- AHRS_M2 msg: 0.09 phi; -0.16 theta; 5.47 psi 2024-11-06T04:36:24Z- handleDepth()- Depth_Keller msg: 1730867784.96 epoch sec; seqNo:168; depth 8.78 2024-11-06T04:36:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867784.98 epoch sec; seqNo:168; 4061513.36 north; 594433.74 east; 36.69 lat; -121.94 lon; 8.78 depth; utmZone: 10 2024-11-06T04:36:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867784.16 2024-11-06T04:36:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:25Z- waiting for 1730867788.16, time is 1730867784.98 2024-11-06T04:36:25Z- AHRS_M2 msg: 1730867785.37 epoch sec; seqNo:151 2024-11-06T04:36:25Z- AHRS_M2 msg: 0.09 phi; -0.17 theta; 5.46 psi 2024-11-06T04:36:25Z- handleDepth()- Depth_Keller msg: 1730867785.37 epoch sec; seqNo:169; depth 8.83 2024-11-06T04:36:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867785.38 epoch sec; seqNo:169; 4061513.36 north; 594433.74 east; 36.69 lat; -121.94 lon; 8.83 depth; utmZone: 10 2024-11-06T04:36:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867784.16 2024-11-06T04:36:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:25Z- waiting for 1730867788.16, time is 1730867785.38 2024-11-06T04:36:25Z- AHRS_M2 msg: 1730867785.77 epoch sec; seqNo:152 2024-11-06T04:36:25Z- AHRS_M2 msg: 0.09 phi; -0.17 theta; 5.46 psi 2024-11-06T04:36:25Z- handleDepth()- Depth_Keller msg: 1730867785.78 epoch sec; seqNo:170; depth 8.88 2024-11-06T04:36:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867785.80 epoch sec; seqNo:170; 4061513.72 north; 594432.52 east; 36.69 lat; -121.94 lon; 8.88 depth; utmZone: 10 2024-11-06T04:36:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867784.16 2024-11-06T04:36:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:25Z- waiting for 1730867788.16, time is 1730867785.80 2024-11-06T04:36:26Z- AHRS_M2 msg: 1730867786.18 epoch sec; seqNo:153 2024-11-06T04:36:26Z- AHRS_M2 msg: 0.10 phi; -0.18 theta; 5.46 psi 2024-11-06T04:36:26Z- handleDepth()- Depth_Keller msg: 1730867786.19 epoch sec; seqNo:171; depth 8.95 2024-11-06T04:36:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867786.21 epoch sec; seqNo:171; 4061514.10 north; 594432.51 east; 36.69 lat; -121.94 lon; 8.95 depth; utmZone: 10 2024-11-06T04:36:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867784.16 2024-11-06T04:36:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:26Z- waiting for 1730867788.16, time is 1730867786.21 2024-11-06T04:36:26Z- AHRS_M2 msg: 1730867786.60 epoch sec; seqNo:154 2024-11-06T04:36:26Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.46 psi 2024-11-06T04:36:26Z- handleDepth()- Depth_Keller msg: 1730867786.60 epoch sec; seqNo:172; depth 9.00 2024-11-06T04:36:26Z- handleDVL() - DVL msg epochMillisec: 1730867786.65 2024-11-06T04:36:26Z- handleDvl() - RDI_Pathfinder msg: 1730867786.65 sec; seqNo:59; #beams:4 2024-11-06T04:36:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.88 , 74.01 , 74.83 , 91.10 2024-11-06T04:36:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.19 , 0.13 2024-11-06T04:36:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867786.71 epoch sec; seqNo:172; 4061514.10 north; 594432.51 east; 36.69 lat; -121.94 lon; 9.00 depth; utmZone: 10 2024-11-06T04:36:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867784.16 2024-11-06T04:36:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:26Z- waiting for 1730867788.16, time is 1730867786.71 2024-11-06T04:36:26Z- AHRS_M2 msg: 1730867786.99 epoch sec; seqNo:155 2024-11-06T04:36:26Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.46 psi 2024-11-06T04:36:26Z- handleDepth()- Depth_Keller msg: 1730867786.99 epoch sec; seqNo:173; depth 9.08 2024-11-06T04:36:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867787.01 epoch sec; seqNo:173; 4061514.47 north; 594431.29 east; 36.69 lat; -121.94 lon; 9.08 depth; utmZone: 10 2024-11-06T04:36:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867784.16 2024-11-06T04:36:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:27Z- waiting for 1730867788.16, time is 1730867787.01 2024-11-06T04:36:27Z- AHRS_M2 msg: 1730867787.40 epoch sec; seqNo:156 2024-11-06T04:36:27Z- AHRS_M2 msg: 0.13 phi; -0.19 theta; 5.46 psi 2024-11-06T04:36:27Z- handleDepth()- Depth_Keller msg: 1730867787.40 epoch sec; seqNo:174; depth 9.11 2024-11-06T04:36:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867787.41 epoch sec; seqNo:174; 4061514.47 north; 594431.29 east; 36.69 lat; -121.94 lon; 9.11 depth; utmZone: 10 2024-11-06T04:36:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867784.16 2024-11-06T04:36:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:27Z- waiting for 1730867788.16, time is 1730867787.41 2024-11-06T04:36:27Z- AHRS_M2 msg: 1730867787.80 epoch sec; seqNo:157 2024-11-06T04:36:27Z- AHRS_M2 msg: 0.14 phi; -0.19 theta; 5.47 psi 2024-11-06T04:36:27Z- handleDepth()- Depth_Keller msg: 1730867787.80 epoch sec; seqNo:175; depth 9.18 2024-11-06T04:36:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867787.81 epoch sec; seqNo:175; 4061514.85 north; 594431.28 east; 36.69 lat; -121.94 lon; 9.18 depth; utmZone: 10 2024-11-06T04:36:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867784.16 2024-11-06T04:36:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:27Z- waiting for 1730867788.16, time is 1730867787.81 2024-11-06T04:36:28Z- AHRS_M2 msg: 1730867788.20 epoch sec; seqNo:158 2024-11-06T04:36:28Z- AHRS_M2 msg: 0.14 phi; -0.19 theta; 5.48 psi 2024-11-06T04:36:28Z- handleDepth()- Depth_Keller msg: 1730867788.21 epoch sec; seqNo:176; depth 9.21 2024-11-06T04:36:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867788.22 epoch sec; seqNo:176; 4061514.85 north; 594431.28 east; 36.69 lat; -121.94 lon; 9.21 depth; utmZone: 10 2024-11-06T04:36:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867784.16 2024-11-06T04:36:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:28Z- waiting for 1730867788.16, time is 1730867788.22 2024-11-06T04:36:28Z- LcmTrn::updateTrn() >>>> pose time 1730867788.22, meas time 1730867786.65 <<<< 2024-11-06T04:36:28Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867788.22 2024-11-06T04:36:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867786.65 sec, ping # 0; waiting for INS data more recent than 1730867784.16... 2024-11-06T04:36:28Z- TerrainNav::motionUpdate() - currEstimate = (4061515, 594431.28, 9.21) 2024-11-06T04:36:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:36:28Z- TerrainNav:: SubcloudNIS is 0.10, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:36:28Z- TNavPF::Projecting Measurements 2024-11-06T04:36:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:36:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:36:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:36:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:36:28Z- TNavFilter::4 good beams. 2024-11-06T04:36:28Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:36:28Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:36:28Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:36:28Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:36:28Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:36:28Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:36:28Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:36:28Z- TerrainMapOctree: (vehN, vehE ) = (4061513.98, 594432.24). 2024-11-06T04:36:28Z- TerrainMapOctree: Min Distance = 298.19. 2024-11-06T04:36:28Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:36:28Z- TerrainMapOctree: Switching to tile 4. 2024-11-06T04:36:29Z- TerrainMapOctree::Octree tile load /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile4.bo took 1.185109 seconds. 2024-11-06T04:36:29Z- Forcing Subcloud Comparison 2024-11-06T04:36:29Z- Weighting particles with subcloud comparison 2024-11-06T04:36:29Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:29Z- meanExpectedMeasDiff: -2.163617 delta_rms_squared: 5.988242 alpha: 0.996671 2024-11-06T04:36:29Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:29Z- meanExpectedMeasDiff: -2.071004 delta_rms_squared: 4.478706 alpha: 0.995554 2024-11-06T04:36:29Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:29Z- meanExpectedMeasDiff: 0.481688 delta_rms_squared: 5.550281 alpha: 0.996410 2024-11-06T04:36:29Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:29Z- meanExpectedMeasDiff: -1.687881 delta_rms_squared: 6.061145 alpha: 0.996711 2024-11-06T04:36:29Z- Subcloud NIS: 0.115870 2024-11-06T04:36:29Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:36:29Z- TNavPF:: sumWeights = 0.434822 2024-11-06T04:36:29Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:36:29Z- TNavPF::First Term of Map Covariance Matrix is 1.34 2024-11-06T04:36:29Z- TNavPF::Calculated NIS Value : 0.07 numBeams normalized NIS: 0.02 2024-11-06T04:36:29Z- TerrainNav::Resampling particle filter... 2024-11-06T04:36:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867786.65 sec. 2024-11-06T04:36:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:36:29Z- variances([0], [2], and psi_berg) = 299.396429 38.829359 0.000000 2024-11-06T04:36:29Z- publishEstimatesDecoder() - at 1730867788.22 sec 2024-11-06T04:36:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:36:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061567.21 594474.50 17.60 2024-11-06T04:36:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061550.68 594482.14 17.06 lat/lon: 36.69 -121.94 2024-11-06T04:36:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 299.40 38.83 1.62 0.00 2024-11-06T04:36:29Z- AHRS_M2 msg: 1730867788.60 epoch sec; seqNo:159 2024-11-06T04:36:29Z- AHRS_M2 msg: 0.15 phi; -0.19 theta; 5.48 psi 2024-11-06T04:36:29Z- handleDepth()- Depth_Keller msg: 1730867788.61 epoch sec; seqNo:177; depth 9.27 2024-11-06T04:36:29Z- handleDVL() - DVL msg epochMillisec: 1730867788.64 2024-11-06T04:36:29Z- handleDvl() - RDI_Pathfinder msg: 1730867788.64 sec; seqNo:60; #beams:4 2024-11-06T04:36:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 93.53 , 73.19 , 74.42 , 92.32 2024-11-06T04:36:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.12 , 0.06 2024-11-06T04:36:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867788.65 epoch sec; seqNo:177; 4061515.21 north; 594430.06 east; 36.69 lat; -121.94 lon; 9.27 depth; utmZone: 10 2024-11-06T04:36:29Z- AHRS_M2 msg: 1730867789.01 epoch sec; seqNo:160 2024-11-06T04:36:29Z- AHRS_M2 msg: 0.15 phi; -0.19 theta; 5.48 psi 2024-11-06T04:36:29Z- handleDepth()- Depth_Keller msg: 1730867789.01 epoch sec; seqNo:178; depth 9.34 2024-11-06T04:36:29Z- handleDVL() - DVL msg epochMillisec: 1730867789.01 2024-11-06T04:36:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:29Z- handleDvl() - RDI_Pathfinder msg: 1730867789.01 sec; seqNo:61; #beams:0 2024-11-06T04:36:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.12 , 0.06 2024-11-06T04:36:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867789.03 epoch sec; seqNo:178; 4061515.21 north; 594430.06 east; 36.69 lat; -121.94 lon; 9.34 depth; utmZone: 10 2024-11-06T04:36:29Z- AHRS_M2 msg: 1730867789.42 epoch sec; seqNo:161 2024-11-06T04:36:29Z- AHRS_M2 msg: 0.14 phi; -0.19 theta; 5.48 psi 2024-11-06T04:36:29Z- handleDepth()- Depth_Keller msg: 1730867789.42 epoch sec; seqNo:179; depth 9.38 2024-11-06T04:36:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867789.43 epoch sec; seqNo:179; 4061515.59 north; 594430.06 east; 36.69 lat; -121.94 lon; 9.38 depth; utmZone: 10 2024-11-06T04:36:29Z- AHRS_M2 msg: 1730867789.82 epoch sec; seqNo:162 2024-11-06T04:36:29Z- AHRS_M2 msg: 0.14 phi; -0.19 theta; 5.48 psi 2024-11-06T04:36:29Z- handleDepth()- Depth_Keller msg: 1730867789.83 epoch sec; seqNo:180; depth 9.44 2024-11-06T04:36:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867789.84 epoch sec; seqNo:180; 4061515.59 north; 594430.06 east; 36.69 lat; -121.94 lon; 9.44 depth; utmZone: 10 2024-11-06T04:36:30Z- 14 msgs in 200 ms and 15 handle calls 2024-11-06T04:36:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867788.22 2024-11-06T04:36:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:30Z- waiting for 1730867792.22, time is 1730867789.84 2024-11-06T04:36:30Z- AHRS_M2 msg: 1730867790.22 epoch sec; seqNo:163 2024-11-06T04:36:30Z- AHRS_M2 msg: 0.14 phi; -0.20 theta; 5.48 psi 2024-11-06T04:36:30Z- handleDepth()- Depth_Keller msg: 1730867790.23 epoch sec; seqNo:181; depth 9.48 2024-11-06T04:36:30Z- handleDVL() - DVL msg epochMillisec: 1730867790.23 2024-11-06T04:36:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:30Z- handleDvl() - RDI_Pathfinder msg: 1730867790.23 sec; seqNo:62; #beams:0 2024-11-06T04:36:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.12 , 0.06 2024-11-06T04:36:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867790.25 epoch sec; seqNo:181; 4061515.96 north; 594428.83 east; 36.69 lat; -121.94 lon; 9.48 depth; utmZone: 10 2024-11-06T04:36:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867788.22 2024-11-06T04:36:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:30Z- waiting for 1730867792.22, time is 1730867790.25 2024-11-06T04:36:30Z- AHRS_M2 msg: 1730867790.62 epoch sec; seqNo:164 2024-11-06T04:36:30Z- AHRS_M2 msg: 0.13 phi; -0.20 theta; 5.48 psi 2024-11-06T04:36:30Z- handleDepth()- Depth_Keller msg: 1730867790.63 epoch sec; seqNo:182; depth 9.54 2024-11-06T04:36:30Z- handleDVL() - DVL msg epochMillisec: 1730867790.65 2024-11-06T04:36:30Z- handleDvl() - RDI_Pathfinder msg: 1730867790.65 sec; seqNo:63; #beams:4 2024-11-06T04:36:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 94.68 , 72.78 , 74.83 , 93.46 2024-11-06T04:36:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.09 , -0.02 2024-11-06T04:36:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867790.69 epoch sec; seqNo:182; 4061515.96 north; 594428.83 east; 36.69 lat; -121.94 lon; 9.54 depth; utmZone: 10 2024-11-06T04:36:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867788.22 2024-11-06T04:36:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:30Z- waiting for 1730867792.22, time is 1730867790.69 2024-11-06T04:36:31Z- AHRS_M2 msg: 1730867791.03 epoch sec; seqNo:165 2024-11-06T04:36:31Z- AHRS_M2 msg: 0.13 phi; -0.20 theta; 5.48 psi 2024-11-06T04:36:31Z- handleDepth()- Depth_Keller msg: 1730867791.03 epoch sec; seqNo:183; depth 9.60 2024-11-06T04:36:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867791.05 epoch sec; seqNo:183; 4061516.34 north; 594428.83 east; 36.69 lat; -121.94 lon; 9.60 depth; utmZone: 10 2024-11-06T04:36:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867788.22 2024-11-06T04:36:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:31Z- waiting for 1730867792.22, time is 1730867791.05 2024-11-06T04:36:31Z- AHRS_M2 msg: 1730867791.43 epoch sec; seqNo:166 2024-11-06T04:36:31Z- AHRS_M2 msg: 0.13 phi; -0.20 theta; 5.48 psi 2024-11-06T04:36:31Z- handleDepth()- Depth_Keller msg: 1730867791.43 epoch sec; seqNo:184; depth 9.66 2024-11-06T04:36:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867791.45 epoch sec; seqNo:184; 4061516.72 north; 594428.82 east; 36.69 lat; -121.94 lon; 9.66 depth; utmZone: 10 2024-11-06T04:36:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867788.22 2024-11-06T04:36:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:31Z- waiting for 1730867792.22, time is 1730867791.45 2024-11-06T04:36:31Z- AHRS_M2 msg: 1730867791.84 epoch sec; seqNo:167 2024-11-06T04:36:31Z- AHRS_M2 msg: 0.13 phi; -0.20 theta; 5.48 psi 2024-11-06T04:36:31Z- handleDepth()- Depth_Keller msg: 1730867791.85 epoch sec; seqNo:185; depth 9.71 2024-11-06T04:36:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867791.86 epoch sec; seqNo:185; 4061516.72 north; 594428.82 east; 36.69 lat; -121.94 lon; 9.71 depth; utmZone: 10 2024-11-06T04:36:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867788.22 2024-11-06T04:36:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:32Z- waiting for 1730867792.22, time is 1730867791.86 2024-11-06T04:36:32Z- AHRS_M2 msg: 1730867792.24 epoch sec; seqNo:168 2024-11-06T04:36:32Z- AHRS_M2 msg: 0.13 phi; -0.20 theta; 5.48 psi 2024-11-06T04:36:32Z- handleDepth()- Depth_Keller msg: 1730867792.25 epoch sec; seqNo:186; depth 9.78 2024-11-06T04:36:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867792.25 epoch sec; seqNo:186; 4061517.08 north; 594427.60 east; 36.69 lat; -121.94 lon; 9.78 depth; utmZone: 10 2024-11-06T04:36:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867788.22 2024-11-06T04:36:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:32Z- waiting for 1730867792.22, time is 1730867792.25 2024-11-06T04:36:32Z- LcmTrn::updateTrn() >>>> pose time 1730867792.25, meas time 1730867790.65 <<<< 2024-11-06T04:36:32Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867792.25 2024-11-06T04:36:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867790.65 sec, ping # 0; waiting for INS data more recent than 1730867788.22... 2024-11-06T04:36:32Z- TerrainNav::motionUpdate() - currEstimate = (4061517, 594427.60, 9.78) 2024-11-06T04:36:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:36:32Z- TerrainNav:: SubcloudNIS is 0.12, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:36:32Z- TNavPF::Projecting Measurements 2024-11-06T04:36:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:36:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:36:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:36:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:36:32Z- TNavFilter::4 good beams. 2024-11-06T04:36:32Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:36:32Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:36:32Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:36:32Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:36:32Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:36:32Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:36:32Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:36:32Z- TerrainMapOctree: (vehN, vehE ) = (4061516.19, 594429.06). 2024-11-06T04:36:32Z- TerrainMapOctree: Min Distance = 294.50. 2024-11-06T04:36:32Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:36:32Z- Forcing Subcloud Comparison 2024-11-06T04:36:32Z- Weighting particles with subcloud comparison 2024-11-06T04:36:32Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:32Z- meanExpectedMeasDiff: 3.243049 delta_rms_squared: 3.491650 alpha: 0.994305 2024-11-06T04:36:32Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:32Z- meanExpectedMeasDiff: -1.998948 delta_rms_squared: 2.481061 alpha: 0.992003 2024-11-06T04:36:32Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:32Z- meanExpectedMeasDiff: 2.341783 delta_rms_squared: 3.553025 alpha: 0.994403 2024-11-06T04:36:32Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:32Z- meanExpectedMeasDiff: 0.548296 delta_rms_squared: 3.700087 alpha: 0.994624 2024-11-06T04:36:32Z- Subcloud NIS: 0.124206 2024-11-06T04:36:32Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:36:32Z- TNavPF:: sumWeights = 0.355009 2024-11-06T04:36:32Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:36:32Z- TNavPF::First Term of Map Covariance Matrix is 1.81 2024-11-06T04:36:32Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T04:36:32Z- TerrainNav::Resampling particle filter... 2024-11-06T04:36:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867790.65 sec. 2024-11-06T04:36:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:36:32Z- variances([0], [2], and psi_berg) = 250.025577 39.370478 0.000000 2024-11-06T04:36:32Z- publishEstimatesDecoder() - at 1730867792.25 sec 2024-11-06T04:36:32Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:36:32Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061569.42 594470.83 17.17 2024-11-06T04:36:32Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061547.90 594479.16 17.37 lat/lon: 36.69 -121.94 2024-11-06T04:36:32Z- LcmTrn::publishEstimatesDecoder() - COVAR: 250.03 39.37 1.38 0.00 2024-11-06T04:36:32Z- AHRS_M2 msg: 1730867792.65 epoch sec; seqNo:169 2024-11-06T04:36:32Z- AHRS_M2 msg: 0.13 phi; -0.20 theta; 5.48 psi 2024-11-06T04:36:32Z- handleDepth()- Depth_Keller msg: 1730867792.66 epoch sec; seqNo:187; depth 9.83 2024-11-06T04:36:32Z- handleDVL() - DVL msg epochMillisec: 1730867792.71 2024-11-06T04:36:32Z- handleDvl() - RDI_Pathfinder msg: 1730867792.71 sec; seqNo:64; #beams:4 2024-11-06T04:36:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.25 , 72.37 , 74.01 , 92.25 2024-11-06T04:36:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.11 , -0.05 2024-11-06T04:36:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867792.74 epoch sec; seqNo:187; 4061517.08 north; 594427.60 east; 36.69 lat; -121.94 lon; 9.83 depth; utmZone: 10 2024-11-06T04:36:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867792.25 2024-11-06T04:36:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:32Z- waiting for 1730867796.25, time is 1730867792.74 2024-11-06T04:36:33Z- AHRS_M2 msg: 1730867793.05 epoch sec; seqNo:170 2024-11-06T04:36:33Z- AHRS_M2 msg: 0.13 phi; -0.20 theta; 5.48 psi 2024-11-06T04:36:33Z- handleDepth()- Depth_Keller msg: 1730867793.05 epoch sec; seqNo:188; depth 9.91 2024-11-06T04:36:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867793.08 epoch sec; seqNo:188; 4061517.46 north; 594427.59 east; 36.69 lat; -121.94 lon; 9.91 depth; utmZone: 10 2024-11-06T04:36:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867792.25 2024-11-06T04:36:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:33Z- waiting for 1730867796.25, time is 1730867793.08 2024-11-06T04:36:33Z- AHRS_M2 msg: 1730867793.45 epoch sec; seqNo:171 2024-11-06T04:36:33Z- AHRS_M2 msg: 0.13 phi; -0.20 theta; 5.48 psi 2024-11-06T04:36:33Z- handleDepth()- Depth_Keller msg: 1730867793.46 epoch sec; seqNo:189; depth 9.95 2024-11-06T04:36:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867793.47 epoch sec; seqNo:189; 4061517.46 north; 594427.59 east; 36.69 lat; -121.94 lon; 9.95 depth; utmZone: 10 2024-11-06T04:36:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867792.25 2024-11-06T04:36:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:33Z- waiting for 1730867796.25, time is 1730867793.47 2024-11-06T04:36:33Z- AHRS_M2 msg: 1730867793.86 epoch sec; seqNo:172 2024-11-06T04:36:33Z- AHRS_M2 msg: 0.13 phi; -0.20 theta; 5.48 psi 2024-11-06T04:36:33Z- handleDepth()- Depth_Keller msg: 1730867793.87 epoch sec; seqNo:190; depth 10.00 2024-11-06T04:36:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867793.88 epoch sec; seqNo:190; 4061517.83 north; 594426.37 east; 36.69 lat; -121.94 lon; 10.00 depth; utmZone: 10 2024-11-06T04:36:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867792.25 2024-11-06T04:36:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:34Z- waiting for 1730867796.25, time is 1730867793.88 2024-11-06T04:36:34Z- AHRS_M2 msg: 1730867794.26 epoch sec; seqNo:173 2024-11-06T04:36:34Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.48 psi 2024-11-06T04:36:34Z- handleDepth()- Depth_Keller msg: 1730867794.27 epoch sec; seqNo:191; depth 10.06 2024-11-06T04:36:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867794.28 epoch sec; seqNo:191; 4061518.20 north; 594426.37 east; 36.69 lat; -121.94 lon; 10.06 depth; utmZone: 10 2024-11-06T04:36:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867792.25 2024-11-06T04:36:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:34Z- waiting for 1730867796.25, time is 1730867794.28 2024-11-06T04:36:34Z- AHRS_M2 msg: 1730867794.67 epoch sec; seqNo:174 2024-11-06T04:36:34Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.48 psi 2024-11-06T04:36:34Z- handleDepth()- Depth_Keller msg: 1730867794.69 epoch sec; seqNo:192; depth 10.11 2024-11-06T04:36:34Z- handleDVL() - DVL msg epochMillisec: 1730867794.71 2024-11-06T04:36:34Z- handleDvl() - RDI_Pathfinder msg: 1730867794.71 sec; seqNo:65; #beams:4 2024-11-06T04:36:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.10 , 72.37 , 72.37 , 92.32 2024-11-06T04:36:34Z- handleDvl() - RDI_Pathfinder msg: velocities 1.08 , -0.15 , -0.06 2024-11-06T04:36:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867794.73 epoch sec; seqNo:192; 4061518.20 north; 594426.37 east; 36.69 lat; -121.94 lon; 10.11 depth; utmZone: 10 2024-11-06T04:36:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867792.25 2024-11-06T04:36:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:34Z- waiting for 1730867796.25, time is 1730867794.73 2024-11-06T04:36:35Z- AHRS_M2 msg: 1730867795.07 epoch sec; seqNo:175 2024-11-06T04:36:35Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.47 psi 2024-11-06T04:36:35Z- handleDepth()- Depth_Keller msg: 1730867795.07 epoch sec; seqNo:193; depth 10.17 2024-11-06T04:36:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867795.10 epoch sec; seqNo:193; 4061518.58 north; 594426.36 east; 36.69 lat; -121.94 lon; 10.17 depth; utmZone: 10 2024-11-06T04:36:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867792.25 2024-11-06T04:36:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:35Z- waiting for 1730867796.25, time is 1730867795.10 2024-11-06T04:36:35Z- AHRS_M2 msg: 1730867795.47 epoch sec; seqNo:176 2024-11-06T04:36:35Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.47 psi 2024-11-06T04:36:35Z- handleDepth()- Depth_Keller msg: 1730867795.48 epoch sec; seqNo:194; depth 10.24 2024-11-06T04:36:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867795.49 epoch sec; seqNo:194; 4061518.95 north; 594425.14 east; 36.69 lat; -121.94 lon; 10.24 depth; utmZone: 10 2024-11-06T04:36:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867792.25 2024-11-06T04:36:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:35Z- waiting for 1730867796.25, time is 1730867795.49 2024-11-06T04:36:35Z- AHRS_M2 msg: 1730867795.88 epoch sec; seqNo:177 2024-11-06T04:36:35Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.47 psi 2024-11-06T04:36:35Z- handleDepth()- Depth_Keller msg: 1730867795.88 epoch sec; seqNo:195; depth 10.30 2024-11-06T04:36:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867795.90 epoch sec; seqNo:195; 4061518.95 north; 594425.14 east; 36.69 lat; -121.94 lon; 10.30 depth; utmZone: 10 2024-11-06T04:36:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867792.25 2024-11-06T04:36:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:36Z- waiting for 1730867796.25, time is 1730867795.90 2024-11-06T04:36:36Z- AHRS_M2 msg: 1730867796.28 epoch sec; seqNo:178 2024-11-06T04:36:36Z- AHRS_M2 msg: 0.13 phi; -0.21 theta; 5.47 psi 2024-11-06T04:36:36Z- handleDepth()- Depth_Keller msg: 1730867796.29 epoch sec; seqNo:196; depth 10.37 2024-11-06T04:36:36Z- handleDVL() - DVL msg epochMillisec: 1730867796.29 2024-11-06T04:36:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:36Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:36Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:36Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:36Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:36Z- handleDvl() - RDI_Pathfinder msg: 1730867796.29 sec; seqNo:66; #beams:0 2024-11-06T04:36:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:36Z- handleDvl() - RDI_Pathfinder msg: velocities 1.08 , -0.15 , -0.06 2024-11-06T04:36:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867796.30 epoch sec; seqNo:196; 4061519.33 north; 594425.13 east; 36.69 lat; -121.94 lon; 10.37 depth; utmZone: 10 2024-11-06T04:36:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867792.25 2024-11-06T04:36:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:36Z- waiting for 1730867796.25, time is 1730867796.30 2024-11-06T04:36:36Z- LcmTrn::updateTrn() >>>> pose time 1730867796.30, meas time 1730867796.29 <<<< 2024-11-06T04:36:36Z- LcmTrn::updateTrn() equating pose 1730867796.29 and meas times 1730867796.29 2024-11-06T04:36:36Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867796.29 2024-11-06T04:36:36Z- TerrainNav::motionUpdate() - currEstimate = (4061519, 594425.13, 10.37) 2024-11-06T04:36:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:36:36Z- TerrainNav:: SubcloudNIS is 0.12, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:36:36Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:36:36Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:36:36Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:36:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:36:36Z- variances([0], [2], and psi_berg) = 250.042386 39.375216 0.000000 2024-11-06T04:36:36Z- publishEstimatesDecoder() - at 1730867796.29 sec 2024-11-06T04:36:36Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:36:36Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061571.70 594468.33 17.45 2024-11-06T04:36:36Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061550.14 594476.70 17.96 lat/lon: 36.69 -121.94 2024-11-06T04:36:36Z- LcmTrn::publishEstimatesDecoder() - COVAR: 250.04 39.38 1.63 0.00 2024-11-06T04:36:36Z- AHRS_M2 msg: 1730867796.69 epoch sec; seqNo:179 2024-11-06T04:36:36Z- AHRS_M2 msg: 0.13 phi; -0.21 theta; 5.47 psi 2024-11-06T04:36:36Z- handleDepth()- Depth_Keller msg: 1730867796.70 epoch sec; seqNo:197; depth 10.43 2024-11-06T04:36:36Z- handleDVL() - DVL msg epochMillisec: 1730867796.74 2024-11-06T04:36:36Z- handleDvl() - RDI_Pathfinder msg: 1730867796.74 sec; seqNo:67; #beams:4 2024-11-06T04:36:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.30 , 72.37 , 72.37 , 92.38 2024-11-06T04:36:36Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , -0.12 , 0.07 2024-11-06T04:36:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867796.75 epoch sec; seqNo:197; 4061519.69 north; 594423.91 east; 36.69 lat; -121.94 lon; 10.43 depth; utmZone: 10 2024-11-06T04:36:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867796.29 2024-11-06T04:36:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:36Z- waiting for 1730867800.29, time is 1730867796.75 2024-11-06T04:36:37Z- AHRS_M2 msg: 1730867797.09 epoch sec; seqNo:180 2024-11-06T04:36:37Z- AHRS_M2 msg: 0.13 phi; -0.21 theta; 5.46 psi 2024-11-06T04:36:37Z- handleDepth()- Depth_Keller msg: 1730867797.10 epoch sec; seqNo:198; depth 10.53 2024-11-06T04:36:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867797.12 epoch sec; seqNo:198; 4061519.69 north; 594423.91 east; 36.69 lat; -121.94 lon; 10.53 depth; utmZone: 10 2024-11-06T04:36:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867796.29 2024-11-06T04:36:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:37Z- waiting for 1730867800.29, time is 1730867797.12 2024-11-06T04:36:37Z- AHRS_M2 msg: 1730867797.49 epoch sec; seqNo:181 2024-11-06T04:36:37Z- AHRS_M2 msg: 0.13 phi; -0.22 theta; 5.46 psi 2024-11-06T04:36:37Z- handleDepth()- Depth_Keller msg: 1730867797.50 epoch sec; seqNo:199; depth 10.58 2024-11-06T04:36:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867797.51 epoch sec; seqNo:199; 4061520.07 north; 594423.90 east; 36.69 lat; -121.94 lon; 10.58 depth; utmZone: 10 2024-11-06T04:36:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867796.29 2024-11-06T04:36:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:37Z- waiting for 1730867800.29, time is 1730867797.51 2024-11-06T04:36:37Z- AHRS_M2 msg: 1730867797.90 epoch sec; seqNo:182 2024-11-06T04:36:37Z- AHRS_M2 msg: 0.13 phi; -0.22 theta; 5.47 psi 2024-11-06T04:36:37Z- handleDepth()- Depth_Keller msg: 1730867797.90 epoch sec; seqNo:200; depth 10.63 2024-11-06T04:36:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867797.91 epoch sec; seqNo:200; 4061520.45 north; 594423.90 east; 36.69 lat; -121.94 lon; 10.63 depth; utmZone: 10 2024-11-06T04:36:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867796.29 2024-11-06T04:36:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:38Z- waiting for 1730867800.29, time is 1730867797.91 2024-11-06T04:36:38Z- AHRS_M2 msg: 1730867798.30 epoch sec; seqNo:183 2024-11-06T04:36:38Z- AHRS_M2 msg: 0.12 phi; -0.22 theta; 5.46 psi 2024-11-06T04:36:38Z- handleDepth()- Depth_Keller msg: 1730867798.31 epoch sec; seqNo:201; depth 10.70 2024-11-06T04:36:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867798.32 epoch sec; seqNo:201; 4061520.44 north; 594422.68 east; 36.69 lat; -121.94 lon; 10.70 depth; utmZone: 10 2024-11-06T04:36:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867796.29 2024-11-06T04:36:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:38Z- waiting for 1730867800.29, time is 1730867798.32 2024-11-06T04:36:38Z- AHRS_M2 msg: 1730867798.71 epoch sec; seqNo:184 2024-11-06T04:36:38Z- AHRS_M2 msg: 0.12 phi; -0.21 theta; 5.46 psi 2024-11-06T04:36:38Z- handleDepth()- Depth_Keller msg: 1730867798.73 epoch sec; seqNo:202; depth 10.77 2024-11-06T04:36:38Z- handleDVL() - DVL msg epochMillisec: 1730867798.78 2024-11-06T04:36:38Z- handleDvl() - RDI_Pathfinder msg: 1730867798.78 sec; seqNo:68; #beams:4 2024-11-06T04:36:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.38 , 71.15 , 72.37 , 93.46 2024-11-06T04:36:38Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , -0.12 , 0.08 2024-11-06T04:36:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867798.80 epoch sec; seqNo:202; 4061520.82 north; 594422.67 east; 36.69 lat; -121.94 lon; 10.77 depth; utmZone: 10 2024-11-06T04:36:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867796.29 2024-11-06T04:36:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:38Z- waiting for 1730867800.29, time is 1730867798.80 2024-11-06T04:36:39Z- AHRS_M2 msg: 1730867799.11 epoch sec; seqNo:185 2024-11-06T04:36:39Z- AHRS_M2 msg: 0.12 phi; -0.21 theta; 5.46 psi 2024-11-06T04:36:39Z- handleDepth()- Depth_Keller msg: 1730867799.11 epoch sec; seqNo:203; depth 10.85 2024-11-06T04:36:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867799.13 epoch sec; seqNo:203; 4061521.19 north; 594422.67 east; 36.69 lat; -121.94 lon; 10.85 depth; utmZone: 10 2024-11-06T04:36:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867796.29 2024-11-06T04:36:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:39Z- waiting for 1730867800.29, time is 1730867799.13 2024-11-06T04:36:39Z- AHRS_M2 msg: 1730867799.51 epoch sec; seqNo:186 2024-11-06T04:36:39Z- AHRS_M2 msg: 0.12 phi; -0.22 theta; 5.46 psi 2024-11-06T04:36:39Z- handleDepth()- Depth_Keller msg: 1730867799.52 epoch sec; seqNo:204; depth 10.90 2024-11-06T04:36:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867799.53 epoch sec; seqNo:204; 4061521.19 north; 594422.67 east; 36.69 lat; -121.94 lon; 10.90 depth; utmZone: 10 2024-11-06T04:36:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867796.29 2024-11-06T04:36:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:39Z- waiting for 1730867800.29, time is 1730867799.53 2024-11-06T04:36:39Z- AHRS_M2 msg: 1730867799.92 epoch sec; seqNo:187 2024-11-06T04:36:39Z- AHRS_M2 msg: 0.12 phi; -0.22 theta; 5.46 psi 2024-11-06T04:36:39Z- handleDepth()- Depth_Keller msg: 1730867799.92 epoch sec; seqNo:205; depth 10.95 2024-11-06T04:36:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867799.93 epoch sec; seqNo:205; 4061521.56 north; 594421.45 east; 36.69 lat; -121.94 lon; 10.95 depth; utmZone: 10 2024-11-06T04:36:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867796.29 2024-11-06T04:36:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:40Z- waiting for 1730867800.29, time is 1730867799.93 2024-11-06T04:36:40Z- AHRS_M2 msg: 1730867800.33 epoch sec; seqNo:188 2024-11-06T04:36:40Z- AHRS_M2 msg: 0.12 phi; -0.22 theta; 5.46 psi 2024-11-06T04:36:40Z- handleDepth()- Depth_Keller msg: 1730867800.33 epoch sec; seqNo:206; depth 11.05 2024-11-06T04:36:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867800.35 epoch sec; seqNo:206; 4061521.56 north; 594421.45 east; 36.69 lat; -121.94 lon; 11.05 depth; utmZone: 10 2024-11-06T04:36:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867796.29 2024-11-06T04:36:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:40Z- waiting for 1730867800.29, time is 1730867800.35 2024-11-06T04:36:40Z- LcmTrn::updateTrn() >>>> pose time 1730867800.35, meas time 1730867798.78 <<<< 2024-11-06T04:36:40Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867800.35 2024-11-06T04:36:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867798.78 sec, ping # 0; waiting for INS data more recent than 1730867796.29... 2024-11-06T04:36:40Z- TerrainNav::motionUpdate() - currEstimate = (4061522, 594421.45, 11.05) 2024-11-06T04:36:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:36:40Z- TerrainNav:: SubcloudNIS is 0.12, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:36:40Z- TNavPF::Projecting Measurements 2024-11-06T04:36:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:36:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:36:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:36:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:36:40Z- TNavFilter::4 good beams. 2024-11-06T04:36:40Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:36:40Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:36:40Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:36:40Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:36:40Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:36:40Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:36:40Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:36:40Z- TerrainMapOctree: (vehN, vehE ) = (4061520.70, 594422.87). 2024-11-06T04:36:40Z- TerrainMapOctree: Min Distance = 287.17. 2024-11-06T04:36:40Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:36:40Z- Forcing Subcloud Comparison 2024-11-06T04:36:40Z- Weighting particles with subcloud comparison 2024-11-06T04:36:40Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:40Z- meanExpectedMeasDiff: 2.447582 delta_rms_squared: 3.450136 alpha: 0.994237 2024-11-06T04:36:40Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:40Z- meanExpectedMeasDiff: -3.522556 delta_rms_squared: 2.576611 alpha: 0.992298 2024-11-06T04:36:40Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:40Z- meanExpectedMeasDiff: 1.560155 delta_rms_squared: 3.926459 alpha: 0.994932 2024-11-06T04:36:40Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:40Z- meanExpectedMeasDiff: 0.401938 delta_rms_squared: 3.482693 alpha: 0.994290 2024-11-06T04:36:40Z- Subcloud NIS: 0.144992 2024-11-06T04:36:40Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:36:40Z- TNavPF:: sumWeights = 0.293234 2024-11-06T04:36:40Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:36:40Z- TNavPF::First Term of Map Covariance Matrix is 1.37 2024-11-06T04:36:40Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T04:36:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867798.78 sec. 2024-11-06T04:36:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:36:40Z- variances([0], [2], and psi_berg) = 168.965793 36.181764 0.000000 2024-11-06T04:36:40Z- publishEstimatesDecoder() - at 1730867800.35 sec 2024-11-06T04:36:40Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:36:40Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061529.70 594479.89 18.62 2024-11-06T04:36:40Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061547.16 594473.34 18.70 lat/lon: 36.69 -121.94 2024-11-06T04:36:40Z- LcmTrn::publishEstimatesDecoder() - COVAR: 168.97 36.18 1.34 0.00 2024-11-06T04:36:40Z- handleDepth()- Depth_Keller msg: 1730867800.74 epoch sec; seqNo:207; depth 11.12 2024-11-06T04:36:40Z- handleDVL() - DVL msg epochMillisec: 1730867800.80 2024-11-06T04:36:40Z- handleDvl() - RDI_Pathfinder msg: 1730867800.80 sec; seqNo:69; #beams:4 2024-11-06T04:36:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.22 , 71.15 , 71.15 , 93.46 2024-11-06T04:36:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.12 , 0.15 2024-11-06T04:36:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867800.84 epoch sec; seqNo:207; 4061521.94 north; 594421.44 east; 36.69 lat; -121.94 lon; 11.12 depth; utmZone: 10 2024-11-06T04:36:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867800.35 2024-11-06T04:36:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:40Z- waiting for 1730867804.35, time is 1730867800.84 2024-11-06T04:36:41Z- AHRS_M2 msg: 1730867801.13 epoch sec; seqNo:189 2024-11-06T04:36:41Z- AHRS_M2 msg: 0.12 phi; -0.23 theta; 5.45 psi 2024-11-06T04:36:41Z- handleDepth()- Depth_Keller msg: 1730867801.13 epoch sec; seqNo:208; depth 11.19 2024-11-06T04:36:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867801.15 epoch sec; seqNo:208; 4061522.30 north; 594420.22 east; 36.69 lat; -121.94 lon; 11.19 depth; utmZone: 10 2024-11-06T04:36:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867800.35 2024-11-06T04:36:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:41Z- waiting for 1730867804.35, time is 1730867801.15 2024-11-06T04:36:41Z- AHRS_M2 msg: 1730867801.53 epoch sec; seqNo:190 2024-11-06T04:36:41Z- AHRS_M2 msg: 0.12 phi; -0.23 theta; 5.45 psi 2024-11-06T04:36:41Z- handleDepth()- Depth_Keller msg: 1730867801.54 epoch sec; seqNo:209; depth 11.25 2024-11-06T04:36:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867801.55 epoch sec; seqNo:209; 4061522.30 north; 594420.22 east; 36.69 lat; -121.94 lon; 11.25 depth; utmZone: 10 2024-11-06T04:36:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867800.35 2024-11-06T04:36:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:41Z- waiting for 1730867804.35, time is 1730867801.55 2024-11-06T04:36:41Z- AHRS_M2 msg: 1730867801.93 epoch sec; seqNo:191 2024-11-06T04:36:41Z- AHRS_M2 msg: 0.12 phi; -0.23 theta; 5.45 psi 2024-11-06T04:36:41Z- handleDepth()- Depth_Keller msg: 1730867801.94 epoch sec; seqNo:210; depth 11.31 2024-11-06T04:36:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867801.97 epoch sec; seqNo:210; 4061522.68 north; 594420.21 east; 36.69 lat; -121.94 lon; 11.31 depth; utmZone: 10 2024-11-06T04:36:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867800.35 2024-11-06T04:36:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:42Z- waiting for 1730867804.35, time is 1730867801.97 2024-11-06T04:36:42Z- AHRS_M2 msg: 1730867802.35 epoch sec; seqNo:192 2024-11-06T04:36:42Z- AHRS_M2 msg: 0.12 phi; -0.24 theta; 5.46 psi 2024-11-06T04:36:42Z- handleDepth()- Depth_Keller msg: 1730867802.38 epoch sec; seqNo:211; depth 11.39 2024-11-06T04:36:42Z- handleDVL() - DVL msg epochMillisec: 1730867802.39 2024-11-06T04:36:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:42Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:42Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:42Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:42Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:42Z- handleDvl() - RDI_Pathfinder msg: 1730867802.39 sec; seqNo:70; #beams:0 2024-11-06T04:36:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.12 , 0.15 2024-11-06T04:36:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867802.42 epoch sec; seqNo:211; 4061522.68 north; 594420.21 east; 36.69 lat; -121.94 lon; 11.39 depth; utmZone: 10 2024-11-06T04:36:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867800.35 2024-11-06T04:36:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:42Z- waiting for 1730867804.35, time is 1730867802.42 2024-11-06T04:36:42Z- handleDepth()- Depth_Keller msg: 1730867802.76 epoch sec; seqNo:212; depth 11.46 2024-11-06T04:36:42Z- handleDVL() - DVL msg epochMillisec: 1730867802.79 2024-11-06T04:36:42Z- handleDvl() - RDI_Pathfinder msg: 1730867802.79 sec; seqNo:71; #beams:4 2024-11-06T04:36:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.37 , 71.15 , 71.96 , 93.53 2024-11-06T04:36:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , 0.05 2024-11-06T04:36:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867802.82 epoch sec; seqNo:212; 4061523.05 north; 594418.99 east; 36.69 lat; -121.94 lon; 11.46 depth; utmZone: 10 2024-11-06T04:36:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867800.35 2024-11-06T04:36:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:42Z- waiting for 1730867804.35, time is 1730867802.82 2024-11-06T04:36:43Z- AHRS_M2 msg: 1730867803.15 epoch sec; seqNo:193 2024-11-06T04:36:43Z- AHRS_M2 msg: 0.12 phi; -0.23 theta; 5.48 psi 2024-11-06T04:36:43Z- handleDepth()- Depth_Keller msg: 1730867803.16 epoch sec; seqNo:213; depth 11.54 2024-11-06T04:36:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867803.18 epoch sec; seqNo:213; 4061523.05 north; 594418.99 east; 36.69 lat; -121.94 lon; 11.54 depth; utmZone: 10 2024-11-06T04:36:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867800.35 2024-11-06T04:36:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:43Z- waiting for 1730867804.35, time is 1730867803.18 2024-11-06T04:36:43Z- AHRS_M2 msg: 1730867803.56 epoch sec; seqNo:194 2024-11-06T04:36:43Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.48 psi 2024-11-06T04:36:43Z- handleDepth()- Depth_Keller msg: 1730867803.57 epoch sec; seqNo:214; depth 11.62 2024-11-06T04:36:43Z- handleDVL() - DVL msg epochMillisec: 1730867803.57 2024-11-06T04:36:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:43Z- handleDvl() - RDI_Pathfinder msg: 1730867803.57 sec; seqNo:72; #beams:0 2024-11-06T04:36:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , 0.05 2024-11-06T04:36:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867803.58 epoch sec; seqNo:214; 4061523.43 north; 594418.98 east; 36.69 lat; -121.94 lon; 11.62 depth; utmZone: 10 2024-11-06T04:36:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867800.35 2024-11-06T04:36:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:43Z- waiting for 1730867804.35, time is 1730867803.58 2024-11-06T04:36:43Z- AHRS_M2 msg: 1730867803.96 epoch sec; seqNo:195 2024-11-06T04:36:43Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.48 psi 2024-11-06T04:36:43Z- handleDepth()- Depth_Keller msg: 1730867803.96 epoch sec; seqNo:215; depth 11.68 2024-11-06T04:36:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867803.97 epoch sec; seqNo:215; 4061523.43 north; 594418.98 east; 36.69 lat; -121.94 lon; 11.68 depth; utmZone: 10 2024-11-06T04:36:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867800.35 2024-11-06T04:36:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:44Z- waiting for 1730867804.35, time is 1730867803.97 2024-11-06T04:36:44Z- AHRS_M2 msg: 1730867804.40 epoch sec; seqNo:196 2024-11-06T04:36:44Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.48 psi 2024-11-06T04:36:44Z- handleDepth()- Depth_Keller msg: 1730867804.41 epoch sec; seqNo:216; depth 11.74 2024-11-06T04:36:44Z- handleDVL() - DVL msg epochMillisec: 1730867804.43 2024-11-06T04:36:44Z- handleDvl() - RDI_Pathfinder msg: 1730867804.43 sec; seqNo:73; #beams:4 2024-11-06T04:36:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 92.38 , 70.74 , 69.92 , 93.46 2024-11-06T04:36:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.06 , -0.03 2024-11-06T04:36:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867804.45 epoch sec; seqNo:216; 4061523.81 north; 594418.98 east; 36.69 lat; -121.94 lon; 11.74 depth; utmZone: 10 2024-11-06T04:36:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867800.35 2024-11-06T04:36:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:44Z- waiting for 1730867804.35, time is 1730867804.45 2024-11-06T04:36:44Z- LcmTrn::updateTrn() >>>> pose time 1730867804.45, meas time 1730867804.43 <<<< 2024-11-06T04:36:44Z- LcmTrn::updateTrn() equating pose 1730867804.43 and meas times 1730867804.43 2024-11-06T04:36:44Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867804.43 2024-11-06T04:36:44Z- TerrainNav::motionUpdate() - currEstimate = (4061524, 594418.98, 11.74) 2024-11-06T04:36:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:36:44Z- TerrainNav:: SubcloudNIS is 0.14, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:36:44Z- TNavPF::Projecting Measurements 2024-11-06T04:36:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:36:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:36:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:36:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:36:44Z- TNavFilter::4 good beams. 2024-11-06T04:36:44Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:36:44Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:36:44Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:36:44Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:36:44Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:36:44Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:36:44Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:36:44Z- TerrainMapOctree: (vehN, vehE ) = (4061523.81, 594418.98). 2024-11-06T04:36:44Z- TerrainMapOctree: Min Distance = 282.34. 2024-11-06T04:36:44Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:36:44Z- Forcing Subcloud Comparison 2024-11-06T04:36:44Z- Weighting particles with subcloud comparison 2024-11-06T04:36:44Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:44Z- meanExpectedMeasDiff: 2.382481 delta_rms_squared: 3.092667 alpha: 0.993575 2024-11-06T04:36:44Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:44Z- meanExpectedMeasDiff: -2.764882 delta_rms_squared: 1.965707 alpha: 0.989928 2024-11-06T04:36:44Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:44Z- meanExpectedMeasDiff: 0.278757 delta_rms_squared: 3.379605 alpha: 0.994117 2024-11-06T04:36:44Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:44Z- meanExpectedMeasDiff: 0.248491 delta_rms_squared: 3.065794 alpha: 0.993519 2024-11-06T04:36:44Z- Subcloud NIS: 0.155275 2024-11-06T04:36:44Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:36:44Z- TNavPF:: sumWeights = 0.398006 2024-11-06T04:36:44Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:36:44Z- TNavPF::First Term of Map Covariance Matrix is 1.60 2024-11-06T04:36:44Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T04:36:44Z- TerrainNav::Resampling particle filter... 2024-11-06T04:36:44Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730867804.43 sec, ping # 0. 2024-11-06T04:36:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:36:44Z- variances([0], [2], and psi_berg) = 132.998251 35.415272 0.000000 2024-11-06T04:36:44Z- publishEstimatesDecoder() - at 1730867804.43 sec 2024-11-06T04:36:44Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:36:44Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061539.73 594459.18 20.24 2024-11-06T04:36:44Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061548.02 594470.53 19.49 lat/lon: 36.69 -121.94 2024-11-06T04:36:44Z- LcmTrn::publishEstimatesDecoder() - COVAR: 133.00 35.42 0.98 0.00 2024-11-06T04:36:44Z- AHRS_M2 msg: 1730867804.81 epoch sec; seqNo:197 2024-11-06T04:36:44Z- AHRS_M2 msg: 0.14 phi; -0.22 theta; 5.48 psi 2024-11-06T04:36:44Z- handleDepth()- Depth_Keller msg: 1730867804.82 epoch sec; seqNo:217; depth 11.84 2024-11-06T04:36:44Z- handleDVL() - DVL msg epochMillisec: 1730867804.83 2024-11-06T04:36:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:44Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:44Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:44Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:44Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:44Z- handleDvl() - RDI_Pathfinder msg: 1730867804.83 sec; seqNo:74; #beams:0 2024-11-06T04:36:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.06 , -0.03 2024-11-06T04:36:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867804.85 epoch sec; seqNo:217; 4061524.17 north; 594417.76 east; 36.69 lat; -121.94 lon; 11.84 depth; utmZone: 10 2024-11-06T04:36:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867804.43 2024-11-06T04:36:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:44Z- waiting for 1730867808.43, time is 1730867804.85 2024-11-06T04:36:45Z- AHRS_M2 msg: 1730867805.17 epoch sec; seqNo:198 2024-11-06T04:36:45Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.48 psi 2024-11-06T04:36:45Z- handleDepth()- Depth_Keller msg: 1730867805.18 epoch sec; seqNo:218; depth 11.91 2024-11-06T04:36:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867805.19 epoch sec; seqNo:218; 4061524.17 north; 594417.76 east; 36.69 lat; -121.94 lon; 11.91 depth; utmZone: 10 2024-11-06T04:36:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867804.43 2024-11-06T04:36:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:45Z- waiting for 1730867808.43, time is 1730867805.19 2024-11-06T04:36:45Z- AHRS_M2 msg: 1730867805.58 epoch sec; seqNo:199 2024-11-06T04:36:45Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.48 psi 2024-11-06T04:36:45Z- handleDepth()- Depth_Keller msg: 1730867805.58 epoch sec; seqNo:219; depth 11.97 2024-11-06T04:36:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867805.59 epoch sec; seqNo:219; 4061524.55 north; 594417.75 east; 36.69 lat; -121.94 lon; 11.97 depth; utmZone: 10 2024-11-06T04:36:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867804.43 2024-11-06T04:36:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:45Z- waiting for 1730867808.43, time is 1730867805.59 2024-11-06T04:36:46Z- handleDepth()- Depth_Keller msg: 1730867806.00 epoch sec; seqNo:220; depth 12.04 2024-11-06T04:36:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867806.01 epoch sec; seqNo:220; 4061524.93 north; 594417.75 east; 36.69 lat; -121.94 lon; 12.04 depth; utmZone: 10 2024-11-06T04:36:46Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:36:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867804.43 2024-11-06T04:36:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:46Z- waiting for 1730867808.43, time is 1730867806.01 2024-11-06T04:36:46Z- AHRS_M2 msg: 1730867806.41 epoch sec; seqNo:200 2024-11-06T04:36:46Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.48 psi 2024-11-06T04:36:46Z- handleDepth()- Depth_Keller msg: 1730867806.42 epoch sec; seqNo:221; depth 12.12 2024-11-06T04:36:46Z- handleDVL() - DVL msg epochMillisec: 1730867806.44 2024-11-06T04:36:46Z- handleDvl() - RDI_Pathfinder msg: 1730867806.44 sec; seqNo:75; #beams:4 2024-11-06T04:36:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 91.30 , 69.10 , 71.96 , 93.46 2024-11-06T04:36:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.11 , 0.05 2024-11-06T04:36:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867806.47 epoch sec; seqNo:221; 4061524.92 north; 594416.53 east; 36.69 lat; -121.94 lon; 12.12 depth; utmZone: 10 2024-11-06T04:36:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867804.43 2024-11-06T04:36:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:46Z- waiting for 1730867808.43, time is 1730867806.47 2024-11-06T04:36:46Z- AHRS_M2 msg: 1730867806.79 epoch sec; seqNo:201 2024-11-06T04:36:46Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.49 psi 2024-11-06T04:36:46Z- handleDepth()- Depth_Keller msg: 1730867806.80 epoch sec; seqNo:222; depth 12.20 2024-11-06T04:36:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867806.82 epoch sec; seqNo:222; 4061525.29 north; 594416.52 east; 36.69 lat; -121.94 lon; 12.20 depth; utmZone: 10 2024-11-06T04:36:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867804.43 2024-11-06T04:36:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:46Z- waiting for 1730867808.43, time is 1730867806.82 2024-11-06T04:36:47Z- AHRS_M2 msg: 1730867807.19 epoch sec; seqNo:202 2024-11-06T04:36:47Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.49 psi 2024-11-06T04:36:47Z- handleDepth()- Depth_Keller msg: 1730867807.20 epoch sec; seqNo:223; depth 12.28 2024-11-06T04:36:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867807.20 epoch sec; seqNo:223; 4061525.29 north; 594416.52 east; 36.69 lat; -121.94 lon; 12.28 depth; utmZone: 10 2024-11-06T04:36:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867804.43 2024-11-06T04:36:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:47Z- waiting for 1730867808.43, time is 1730867807.20 2024-11-06T04:36:47Z- AHRS_M2 msg: 1730867807.61 epoch sec; seqNo:203 2024-11-06T04:36:47Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.50 psi 2024-11-06T04:36:47Z- handleDepth()- Depth_Keller msg: 1730867807.61 epoch sec; seqNo:224; depth 12.34 2024-11-06T04:36:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867807.62 epoch sec; seqNo:224; 4061525.67 north; 594416.52 east; 36.69 lat; -121.94 lon; 12.34 depth; utmZone: 10 2024-11-06T04:36:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867804.43 2024-11-06T04:36:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:47Z- waiting for 1730867808.43, time is 1730867807.62 2024-11-06T04:36:48Z- AHRS_M2 msg: 1730867808.00 epoch sec; seqNo:204 2024-11-06T04:36:48Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.50 psi 2024-11-06T04:36:48Z- handleDepth()- Depth_Keller msg: 1730867808.01 epoch sec; seqNo:225; depth 12.41 2024-11-06T04:36:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867808.02 epoch sec; seqNo:225; 4061525.67 north; 594416.52 east; 36.69 lat; -121.94 lon; 12.41 depth; utmZone: 10 2024-11-06T04:36:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867804.43 2024-11-06T04:36:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:48Z- waiting for 1730867808.43, time is 1730867808.02 2024-11-06T04:36:48Z- AHRS_M2 msg: 1730867808.42 epoch sec; seqNo:205 2024-11-06T04:36:48Z- AHRS_M2 msg: 0.13 phi; -0.22 theta; 5.51 psi 2024-11-06T04:36:48Z- handleDepth()- Depth_Keller msg: 1730867808.42 epoch sec; seqNo:226; depth 12.46 2024-11-06T04:36:48Z- handleDVL() - DVL msg epochMillisec: 1730867808.45 2024-11-06T04:36:48Z- handleDvl() - RDI_Pathfinder msg: 1730867808.45 sec; seqNo:76; #beams:4 2024-11-06T04:36:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 90.28 , 70.32 , 71.14 , 94.61 2024-11-06T04:36:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , -0.15 2024-11-06T04:36:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867808.46 epoch sec; seqNo:226; 4061526.04 north; 594415.29 east; 36.69 lat; -121.94 lon; 12.46 depth; utmZone: 10 2024-11-06T04:36:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867804.43 2024-11-06T04:36:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:48Z- waiting for 1730867808.43, time is 1730867808.46 2024-11-06T04:36:48Z- LcmTrn::updateTrn() >>>> pose time 1730867808.46, meas time 1730867808.45 <<<< 2024-11-06T04:36:48Z- LcmTrn::updateTrn() equating pose 1730867808.45 and meas times 1730867808.45 2024-11-06T04:36:48Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867808.45 2024-11-06T04:36:48Z- TerrainNav::motionUpdate() - currEstimate = (4061526, 594415.29, 12.46) 2024-11-06T04:36:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:36:48Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:36:48Z- TNavPF::Projecting Measurements 2024-11-06T04:36:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:36:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:36:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:36:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:36:48Z- TNavFilter::4 good beams. 2024-11-06T04:36:48Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:36:48Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:36:48Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:36:48Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:36:48Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:36:48Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:36:48Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:36:48Z- TerrainMapOctree: (vehN, vehE ) = (4061526.04, 594415.29). 2024-11-06T04:36:48Z- TerrainMapOctree: Min Distance = 278.35. 2024-11-06T04:36:48Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:36:48Z- Forcing Subcloud Comparison 2024-11-06T04:36:48Z- Weighting particles with subcloud comparison 2024-11-06T04:36:48Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:48Z- meanExpectedMeasDiff: 0.863750 delta_rms_squared: 2.537850 alpha: 0.992181 2024-11-06T04:36:48Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:48Z- meanExpectedMeasDiff: -2.350692 delta_rms_squared: 1.576085 alpha: 0.987469 2024-11-06T04:36:48Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:48Z- meanExpectedMeasDiff: 3.230635 delta_rms_squared: 3.427351 alpha: 0.994198 2024-11-06T04:36:48Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:48Z- meanExpectedMeasDiff: 2.373015 delta_rms_squared: 2.138462 alpha: 0.990734 2024-11-06T04:36:48Z- Subcloud NIS: 0.173310 2024-11-06T04:36:48Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:36:48Z- TNavPF:: sumWeights = 0.296358 2024-11-06T04:36:48Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:36:48Z- TNavPF::First Term of Map Covariance Matrix is 0.56 2024-11-06T04:36:48Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T04:36:48Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730867808.45 sec, ping # 0. 2024-11-06T04:36:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:36:48Z- variances([0], [2], and psi_berg) = 106.702698 25.641300 0.000000 2024-11-06T04:36:48Z- publishEstimatesDecoder() - at 1730867808.45 sec 2024-11-06T04:36:48Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:36:48Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061553.83 594468.93 19.38 2024-11-06T04:36:48Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061547.48 594468.06 20.02 lat/lon: 36.69 -121.94 2024-11-06T04:36:48Z- LcmTrn::publishEstimatesDecoder() - COVAR: 106.70 25.64 0.88 0.00 2024-11-06T04:36:48Z- AHRS_M2 msg: 1730867808.81 epoch sec; seqNo:206 2024-11-06T04:36:48Z- AHRS_M2 msg: 0.13 phi; -0.22 theta; 5.51 psi 2024-11-06T04:36:48Z- handleDepth()- Depth_Keller msg: 1730867808.81 epoch sec; seqNo:227; depth 12.54 2024-11-06T04:36:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867808.86 epoch sec; seqNo:227; 4061526.42 north; 594415.29 east; 36.69 lat; -121.94 lon; 12.54 depth; utmZone: 10 2024-11-06T04:36:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867808.45 2024-11-06T04:36:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:48Z- waiting for 1730867812.45, time is 1730867808.86 2024-11-06T04:36:49Z- handleDepth()- Depth_Keller msg: 1730867809.23 epoch sec; seqNo:228; depth 12.60 2024-11-06T04:36:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867809.24 epoch sec; seqNo:228; 4061526.42 north; 594415.29 east; 36.69 lat; -121.94 lon; 12.60 depth; utmZone: 10 2024-11-06T04:36:49Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:36:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867808.45 2024-11-06T04:36:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:49Z- waiting for 1730867812.45, time is 1730867809.24 2024-11-06T04:36:49Z- AHRS_M2 msg: 1730867809.62 epoch sec; seqNo:207 2024-11-06T04:36:49Z- AHRS_M2 msg: 0.12 phi; -0.22 theta; 5.51 psi 2024-11-06T04:36:49Z- handleDepth()- Depth_Keller msg: 1730867809.62 epoch sec; seqNo:229; depth 12.68 2024-11-06T04:36:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867809.63 epoch sec; seqNo:229; 4061526.80 north; 594415.29 east; 36.69 lat; -121.94 lon; 12.68 depth; utmZone: 10 2024-11-06T04:36:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867808.45 2024-11-06T04:36:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:49Z- waiting for 1730867812.45, time is 1730867809.63 2024-11-06T04:36:50Z- AHRS_M2 msg: 1730867810.02 epoch sec; seqNo:208 2024-11-06T04:36:50Z- AHRS_M2 msg: 0.12 phi; -0.21 theta; 5.50 psi 2024-11-06T04:36:50Z- handleDepth()- Depth_Keller msg: 1730867810.03 epoch sec; seqNo:230; depth 12.76 2024-11-06T04:36:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867810.04 epoch sec; seqNo:230; 4061527.16 north; 594414.06 east; 36.69 lat; -121.94 lon; 12.76 depth; utmZone: 10 2024-11-06T04:36:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867808.45 2024-11-06T04:36:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:50Z- waiting for 1730867812.45, time is 1730867810.04 2024-11-06T04:36:50Z- AHRS_M2 msg: 1730867810.42 epoch sec; seqNo:209 2024-11-06T04:36:50Z- AHRS_M2 msg: 0.11 phi; -0.21 theta; 5.50 psi 2024-11-06T04:36:50Z- handleDepth()- Depth_Keller msg: 1730867810.43 epoch sec; seqNo:231; depth 12.83 2024-11-06T04:36:50Z- handleDVL() - DVL msg epochMillisec: 1730867810.45 2024-11-06T04:36:50Z- handleDvl() - RDI_Pathfinder msg: 1730867810.45 sec; seqNo:77; #beams:4 2024-11-06T04:36:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.20 , 68.28 , 69.92 , 92.45 2024-11-06T04:36:50Z- handleDvl() - RDI_Pathfinder msg: velocities 1.21 , -0.09 , -0.09 2024-11-06T04:36:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867810.47 epoch sec; seqNo:231; 4061527.54 north; 594414.06 east; 36.69 lat; -121.94 lon; 12.83 depth; utmZone: 10 2024-11-06T04:36:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867808.45 2024-11-06T04:36:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:50Z- waiting for 1730867812.45, time is 1730867810.47 2024-11-06T04:36:50Z- AHRS_M2 msg: 1730867810.84 epoch sec; seqNo:210 2024-11-06T04:36:50Z- AHRS_M2 msg: 0.11 phi; -0.21 theta; 5.50 psi 2024-11-06T04:36:50Z- handleDepth()- Depth_Keller msg: 1730867810.84 epoch sec; seqNo:232; depth 12.90 2024-11-06T04:36:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867810.86 epoch sec; seqNo:232; 4061527.92 north; 594414.05 east; 36.69 lat; -121.94 lon; 12.90 depth; utmZone: 10 2024-11-06T04:36:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867808.45 2024-11-06T04:36:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:50Z- waiting for 1730867812.45, time is 1730867810.86 2024-11-06T04:36:51Z- AHRS_M2 msg: 1730867811.23 epoch sec; seqNo:211 2024-11-06T04:36:51Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.50 psi 2024-11-06T04:36:51Z- handleDepth()- Depth_Keller msg: 1730867811.24 epoch sec; seqNo:233; depth 12.96 2024-11-06T04:36:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867811.25 epoch sec; seqNo:233; 4061527.91 north; 594412.83 east; 36.69 lat; -121.94 lon; 12.96 depth; utmZone: 10 2024-11-06T04:36:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867808.45 2024-11-06T04:36:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:51Z- waiting for 1730867812.45, time is 1730867811.25 2024-11-06T04:36:51Z- AHRS_M2 msg: 1730867811.64 epoch sec; seqNo:212 2024-11-06T04:36:51Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.50 psi 2024-11-06T04:36:51Z- handleDepth()- Depth_Keller msg: 1730867811.64 epoch sec; seqNo:234; depth 13.04 2024-11-06T04:36:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867811.65 epoch sec; seqNo:234; 4061528.28 north; 594412.83 east; 36.69 lat; -121.94 lon; 13.04 depth; utmZone: 10 2024-11-06T04:36:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867808.45 2024-11-06T04:36:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:51Z- waiting for 1730867812.45, time is 1730867811.65 2024-11-06T04:36:52Z- AHRS_M2 msg: 1730867812.04 epoch sec; seqNo:213 2024-11-06T04:36:52Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.49 psi 2024-11-06T04:36:52Z- handleDepth()- Depth_Keller msg: 1730867812.05 epoch sec; seqNo:235; depth 13.13 2024-11-06T04:36:52Z- handleDVL() - DVL msg epochMillisec: 1730867812.05 2024-11-06T04:36:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:52Z- handleDvl() - RDI_Pathfinder msg: 1730867812.05 sec; seqNo:78; #beams:0 2024-11-06T04:36:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:52Z- handleDvl() - RDI_Pathfinder msg: velocities 1.21 , -0.09 , -0.09 2024-11-06T04:36:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867812.06 epoch sec; seqNo:235; 4061528.66 north; 594412.82 east; 36.69 lat; -121.94 lon; 13.13 depth; utmZone: 10 2024-11-06T04:36:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867808.45 2024-11-06T04:36:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:52Z- waiting for 1730867812.45, time is 1730867812.06 2024-11-06T04:36:52Z- AHRS_M2 msg: 1730867812.45 epoch sec; seqNo:214 2024-11-06T04:36:52Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.49 psi 2024-11-06T04:36:52Z- handleDepth()- Depth_Keller msg: 1730867812.45 epoch sec; seqNo:236; depth 13.22 2024-11-06T04:36:52Z- handleDVL() - DVL msg epochMillisec: 1730867812.50 2024-11-06T04:36:52Z- handleDvl() - RDI_Pathfinder msg: 1730867812.50 sec; seqNo:79; #beams:4 2024-11-06T04:36:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.17 , 67.46 , 70.32 , 89.33 2024-11-06T04:36:52Z- handleDvl() - RDI_Pathfinder msg: velocities 1.24 , -0.17 , 0.18 2024-11-06T04:36:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867812.53 epoch sec; seqNo:236; 4061529.03 north; 594411.60 east; 36.69 lat; -121.94 lon; 13.22 depth; utmZone: 10 2024-11-06T04:36:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867808.45 2024-11-06T04:36:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:52Z- waiting for 1730867812.45, time is 1730867812.53 2024-11-06T04:36:52Z- LcmTrn::updateTrn() >>>> pose time 1730867812.53, meas time 1730867812.50 <<<< 2024-11-06T04:36:52Z- LcmTrn::updateTrn() equating pose 1730867812.50 and meas times 1730867812.50 2024-11-06T04:36:52Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867812.50 2024-11-06T04:36:52Z- TerrainNav::motionUpdate() - currEstimate = (4061529, 594411.60, 13.22) 2024-11-06T04:36:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:36:52Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:36:52Z- TNavPF::Projecting Measurements 2024-11-06T04:36:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:36:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:36:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:36:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:36:52Z- TNavFilter::4 good beams. 2024-11-06T04:36:52Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:36:52Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:36:52Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:36:52Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:36:52Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:36:52Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:36:52Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:36:52Z- TerrainMapOctree: (vehN, vehE ) = (4061529.03, 594411.60). 2024-11-06T04:36:52Z- TerrainMapOctree: Min Distance = 273.76. 2024-11-06T04:36:52Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:36:52Z- Forcing Subcloud Comparison 2024-11-06T04:36:52Z- Weighting particles with subcloud comparison 2024-11-06T04:36:52Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:52Z- meanExpectedMeasDiff: -0.043759 delta_rms_squared: 2.157758 alpha: 0.990816 2024-11-06T04:36:52Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:52Z- meanExpectedMeasDiff: -4.799672 delta_rms_squared: 1.666184 alpha: 0.988139 2024-11-06T04:36:52Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:52Z- meanExpectedMeasDiff: 2.721801 delta_rms_squared: 3.596154 alpha: 0.994469 2024-11-06T04:36:52Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:52Z- meanExpectedMeasDiff: 0.051320 delta_rms_squared: 1.857693 alpha: 0.989349 2024-11-06T04:36:52Z- Subcloud NIS: 0.190716 2024-11-06T04:36:52Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:36:52Z- TNavPF:: sumWeights = 0.178203 2024-11-06T04:36:52Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:36:52Z- TNavPF::First Term of Map Covariance Matrix is 0.24 2024-11-06T04:36:52Z- TNavPF::Calculated NIS Value : 0.07 numBeams normalized NIS: 0.02 2024-11-06T04:36:52Z- TerrainNav::Resampling particle filter... 2024-11-06T04:36:52Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730867812.50 sec, ping # 0. 2024-11-06T04:36:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:36:52Z- variances([0], [2], and psi_berg) = 42.974520 13.611313 0.000000 2024-11-06T04:36:52Z- publishEstimatesDecoder() - at 1730867812.50 sec 2024-11-06T04:36:52Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:36:52Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061545.17 594451.91 22.35 2024-11-06T04:36:52Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061546.55 594465.25 21.12 lat/lon: 36.69 -121.94 2024-11-06T04:36:52Z- LcmTrn::publishEstimatesDecoder() - COVAR: 42.97 13.61 0.75 0.00 2024-11-06T04:36:52Z- AHRS_M2 msg: 1730867812.87 epoch sec; seqNo:215 2024-11-06T04:36:52Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.49 psi 2024-11-06T04:36:52Z- handleDepth()- Depth_Keller msg: 1730867812.88 epoch sec; seqNo:237; depth 13.31 2024-11-06T04:36:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867812.93 epoch sec; seqNo:237; 4061529.41 north; 594411.60 east; 36.69 lat; -121.94 lon; 13.31 depth; utmZone: 10 2024-11-06T04:36:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867812.50 2024-11-06T04:36:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:53Z- waiting for 1730867816.50, time is 1730867812.93 2024-11-06T04:36:53Z- AHRS_M2 msg: 1730867813.26 epoch sec; seqNo:216 2024-11-06T04:36:53Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.49 psi 2024-11-06T04:36:53Z- handleDepth()- Depth_Keller msg: 1730867813.27 epoch sec; seqNo:238; depth 13.39 2024-11-06T04:36:53Z- handleDVL() - DVL msg epochMillisec: 1730867813.27 2024-11-06T04:36:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:53Z- handleDvl() - RDI_Pathfinder msg: 1730867813.27 sec; seqNo:80; #beams:0 2024-11-06T04:36:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:53Z- handleDvl() - RDI_Pathfinder msg: velocities 1.24 , -0.17 , 0.18 2024-11-06T04:36:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867813.28 epoch sec; seqNo:238; 4061529.41 north; 594411.60 east; 36.69 lat; -121.94 lon; 13.39 depth; utmZone: 10 2024-11-06T04:36:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867812.50 2024-11-06T04:36:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:53Z- waiting for 1730867816.50, time is 1730867813.28 2024-11-06T04:36:53Z- AHRS_M2 msg: 1730867813.66 epoch sec; seqNo:217 2024-11-06T04:36:53Z- AHRS_M2 msg: 0.13 phi; -0.19 theta; 5.49 psi 2024-11-06T04:36:53Z- handleDepth()- Depth_Keller msg: 1730867813.66 epoch sec; seqNo:239; depth 13.45 2024-11-06T04:36:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867813.69 epoch sec; seqNo:239; 4061529.79 north; 594411.59 east; 36.69 lat; -121.94 lon; 13.45 depth; utmZone: 10 2024-11-06T04:36:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867812.50 2024-11-06T04:36:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:53Z- waiting for 1730867816.50, time is 1730867813.69 2024-11-06T04:36:54Z- AHRS_M2 msg: 1730867814.06 epoch sec; seqNo:218 2024-11-06T04:36:54Z- AHRS_M2 msg: 0.13 phi; -0.19 theta; 5.48 psi 2024-11-06T04:36:54Z- handleDepth()- Depth_Keller msg: 1730867814.07 epoch sec; seqNo:240; depth 13.54 2024-11-06T04:36:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867814.08 epoch sec; seqNo:240; 4061530.15 north; 594410.37 east; 36.69 lat; -121.94 lon; 13.54 depth; utmZone: 10 2024-11-06T04:36:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867812.50 2024-11-06T04:36:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:54Z- waiting for 1730867816.50, time is 1730867814.08 2024-11-06T04:36:54Z- AHRS_M2 msg: 1730867814.48 epoch sec; seqNo:219 2024-11-06T04:36:54Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.48 psi 2024-11-06T04:36:54Z- handleDepth()- Depth_Keller msg: 1730867814.48 epoch sec; seqNo:241; depth 13.61 2024-11-06T04:36:54Z- handleDVL() - DVL msg epochMillisec: 1730867814.53 2024-11-06T04:36:54Z- handleDvl() - RDI_Pathfinder msg: 1730867814.53 sec; seqNo:81; #beams:4 2024-11-06T04:36:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 89.34 , 67.47 , 71.15 , 90.42 2024-11-06T04:36:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.13 , 0.30 2024-11-06T04:36:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867814.55 epoch sec; seqNo:241; 4061530.15 north; 594410.37 east; 36.69 lat; -121.94 lon; 13.61 depth; utmZone: 10 2024-11-06T04:36:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867812.50 2024-11-06T04:36:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:54Z- waiting for 1730867816.50, time is 1730867814.55 2024-11-06T04:36:54Z- AHRS_M2 msg: 1730867814.87 epoch sec; seqNo:220 2024-11-06T04:36:54Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.48 psi 2024-11-06T04:36:54Z- handleDepth()- Depth_Keller msg: 1730867814.88 epoch sec; seqNo:242; depth 13.72 2024-11-06T04:36:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867814.91 epoch sec; seqNo:242; 4061530.53 north; 594410.36 east; 36.69 lat; -121.94 lon; 13.72 depth; utmZone: 10 2024-11-06T04:36:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867812.50 2024-11-06T04:36:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:55Z- waiting for 1730867816.50, time is 1730867814.91 2024-11-06T04:36:55Z- handleDepth()- Depth_Keller msg: 1730867815.30 epoch sec; seqNo:243; depth 13.77 2024-11-06T04:36:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867815.31 epoch sec; seqNo:243; 4061530.53 north; 594410.36 east; 36.69 lat; -121.94 lon; 13.77 depth; utmZone: 10 2024-11-06T04:36:55Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:36:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867812.50 2024-11-06T04:36:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:55Z- waiting for 1730867816.50, time is 1730867815.31 2024-11-06T04:36:55Z- AHRS_M2 msg: 1730867815.69 epoch sec; seqNo:221 2024-11-06T04:36:55Z- AHRS_M2 msg: 0.12 phi; -0.22 theta; 5.48 psi 2024-11-06T04:36:55Z- handleDepth()- Depth_Keller msg: 1730867815.69 epoch sec; seqNo:244; depth 13.84 2024-11-06T04:36:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867815.71 epoch sec; seqNo:244; 4061530.90 north; 594409.14 east; 36.69 lat; -121.94 lon; 13.84 depth; utmZone: 10 2024-11-06T04:36:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867812.50 2024-11-06T04:36:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:55Z- waiting for 1730867816.50, time is 1730867815.71 2024-11-06T04:36:56Z- AHRS_M2 msg: 1730867816.08 epoch sec; seqNo:222 2024-11-06T04:36:56Z- AHRS_M2 msg: 0.12 phi; -0.22 theta; 5.48 psi 2024-11-06T04:36:56Z- handleDepth()- Depth_Keller msg: 1730867816.09 epoch sec; seqNo:245; depth 13.92 2024-11-06T04:36:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867816.10 epoch sec; seqNo:245; 4061530.90 north; 594409.14 east; 36.69 lat; -121.94 lon; 13.92 depth; utmZone: 10 2024-11-06T04:36:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867812.50 2024-11-06T04:36:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:56Z- waiting for 1730867816.50, time is 1730867816.10 2024-11-06T04:36:56Z- AHRS_M2 msg: 1730867816.49 epoch sec; seqNo:223 2024-11-06T04:36:56Z- AHRS_M2 msg: 0.12 phi; -0.21 theta; 5.49 psi 2024-11-06T04:36:56Z- handleDepth()- Depth_Keller msg: 1730867816.50 epoch sec; seqNo:246; depth 13.98 2024-11-06T04:36:56Z- handleDVL() - DVL msg epochMillisec: 1730867816.52 2024-11-06T04:36:56Z- handleDvl() - RDI_Pathfinder msg: 1730867816.52 sec; seqNo:82; #beams:4 2024-11-06T04:36:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 88.26 , 67.47 , 70.33 , 92.59 2024-11-06T04:36:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , 0.00 , -0.03 2024-11-06T04:36:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867816.53 epoch sec; seqNo:246; 4061531.28 north; 594409.13 east; 36.69 lat; -121.94 lon; 13.98 depth; utmZone: 10 2024-11-06T04:36:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867812.50 2024-11-06T04:36:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:56Z- waiting for 1730867816.50, time is 1730867816.53 2024-11-06T04:36:56Z- LcmTrn::updateTrn() >>>> pose time 1730867816.53, meas time 1730867816.52 <<<< 2024-11-06T04:36:56Z- LcmTrn::updateTrn() equating pose 1730867816.52 and meas times 1730867816.52 2024-11-06T04:36:56Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867816.52 2024-11-06T04:36:56Z- TerrainNav::motionUpdate() - currEstimate = (4061531, 594409.13, 13.98) 2024-11-06T04:36:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:36:56Z- TerrainNav:: SubcloudNIS is 0.19, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:36:56Z- TNavPF::Projecting Measurements 2024-11-06T04:36:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:36:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:36:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:36:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:36:56Z- TNavFilter::4 good beams. 2024-11-06T04:36:56Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:36:56Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:36:56Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:36:56Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:36:56Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:36:56Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:36:56Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:36:56Z- TerrainMapOctree: (vehN, vehE ) = (4061531.28, 594409.13). 2024-11-06T04:36:56Z- TerrainMapOctree: Min Distance = 270.49. 2024-11-06T04:36:56Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:36:56Z- Forcing Subcloud Comparison 2024-11-06T04:36:56Z- Weighting particles with subcloud comparison 2024-11-06T04:36:56Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:56Z- meanExpectedMeasDiff: 1.491011 delta_rms_squared: 1.736244 alpha: 0.988612 2024-11-06T04:36:56Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:56Z- meanExpectedMeasDiff: -3.671504 delta_rms_squared: 1.439572 alpha: 0.986297 2024-11-06T04:36:56Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:56Z- meanExpectedMeasDiff: 3.654433 delta_rms_squared: 2.202329 alpha: 0.991000 2024-11-06T04:36:56Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:36:56Z- meanExpectedMeasDiff: 2.938220 delta_rms_squared: 1.780830 alpha: 0.988894 2024-11-06T04:36:56Z- Subcloud NIS: 0.206879 2024-11-06T04:36:56Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:36:56Z- TNavPF:: sumWeights = 0.156706 2024-11-06T04:36:56Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:36:56Z- TNavPF::First Term of Map Covariance Matrix is 0.33 2024-11-06T04:36:56Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.02 2024-11-06T04:36:56Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730867816.52 sec, ping # 0. 2024-11-06T04:36:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:36:56Z- variances([0], [2], and psi_berg) = 16.504365 5.978131 0.000000 2024-11-06T04:36:56Z- publishEstimatesDecoder() - at 1730867816.52 sec 2024-11-06T04:36:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:36:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061514.97 594436.37 23.17 2024-11-06T04:36:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061547.74 594463.55 21.74 lat/lon: 36.69 -121.94 2024-11-06T04:36:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 16.50 5.98 0.67 0.00 2024-11-06T04:36:56Z- AHRS_M2 msg: 1730867816.89 epoch sec; seqNo:224 2024-11-06T04:36:56Z- AHRS_M2 msg: 0.12 phi; -0.21 theta; 5.49 psi 2024-11-06T04:36:56Z- handleDepth()- Depth_Keller msg: 1730867816.91 epoch sec; seqNo:247; depth 14.06 2024-11-06T04:36:56Z- handleDVL() - DVL msg epochMillisec: 1730867816.92 2024-11-06T04:36:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:36:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:36:56Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:36:56Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:36:56Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:36:56Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:36:56Z- handleDvl() - RDI_Pathfinder msg: 1730867816.92 sec; seqNo:83; #beams:0 2024-11-06T04:36:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:36:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.72 , 0.00 , -0.03 2024-11-06T04:36:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867816.94 epoch sec; seqNo:247; 4061531.28 north; 594409.13 east; 36.69 lat; -121.94 lon; 14.06 depth; utmZone: 10 2024-11-06T04:36:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867816.52 2024-11-06T04:36:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:57Z- waiting for 1730867820.52, time is 1730867816.94 2024-11-06T04:36:57Z- AHRS_M2 msg: 1730867817.30 epoch sec; seqNo:225 2024-11-06T04:36:57Z- AHRS_M2 msg: 0.12 phi; -0.21 theta; 5.49 psi 2024-11-06T04:36:57Z- handleDepth()- Depth_Keller msg: 1730867817.30 epoch sec; seqNo:248; depth 14.10 2024-11-06T04:36:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867817.31 epoch sec; seqNo:248; 4061531.65 north; 594409.13 east; 36.69 lat; -121.94 lon; 14.10 depth; utmZone: 10 2024-11-06T04:36:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867816.52 2024-11-06T04:36:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:57Z- waiting for 1730867820.52, time is 1730867817.31 2024-11-06T04:36:57Z- AHRS_M2 msg: 1730867817.71 epoch sec; seqNo:226 2024-11-06T04:36:57Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.49 psi 2024-11-06T04:36:57Z- handleDepth()- Depth_Keller msg: 1730867817.71 epoch sec; seqNo:249; depth 14.18 2024-11-06T04:36:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867817.72 epoch sec; seqNo:249; 4061531.64 north; 594407.91 east; 36.69 lat; -121.94 lon; 14.18 depth; utmZone: 10 2024-11-06T04:36:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867816.52 2024-11-06T04:36:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:57Z- waiting for 1730867820.52, time is 1730867817.72 2024-11-06T04:36:58Z- AHRS_M2 msg: 1730867818.11 epoch sec; seqNo:227 2024-11-06T04:36:58Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.49 psi 2024-11-06T04:36:58Z- handleDepth()- Depth_Keller msg: 1730867818.12 epoch sec; seqNo:250; depth 14.24 2024-11-06T04:36:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867818.13 epoch sec; seqNo:250; 4061532.02 north; 594407.91 east; 36.69 lat; -121.94 lon; 14.24 depth; utmZone: 10 2024-11-06T04:36:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867816.52 2024-11-06T04:36:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:58Z- waiting for 1730867820.52, time is 1730867818.13 2024-11-06T04:36:58Z- AHRS_M2 msg: 1730867818.51 epoch sec; seqNo:228 2024-11-06T04:36:58Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.49 psi 2024-11-06T04:36:58Z- handleDepth()- Depth_Keller msg: 1730867818.51 epoch sec; seqNo:251; depth 14.32 2024-11-06T04:36:58Z- handleDVL() - DVL msg epochMillisec: 1730867818.55 2024-11-06T04:36:58Z- handleDvl() - RDI_Pathfinder msg: 1730867818.55 sec; seqNo:84; #beams:4 2024-11-06T04:36:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.17 , 67.06 , 69.92 , 88.26 2024-11-06T04:36:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.07 , -0.12 2024-11-06T04:36:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867818.56 epoch sec; seqNo:251; 4061532.40 north; 594407.90 east; 36.69 lat; -121.94 lon; 14.32 depth; utmZone: 10 2024-11-06T04:36:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:36:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867816.52 2024-11-06T04:36:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:58Z- waiting for 1730867820.52, time is 1730867818.56 2024-11-06T04:36:58Z- AHRS_M2 msg: 1730867818.91 epoch sec; seqNo:229 2024-11-06T04:36:58Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.49 psi 2024-11-06T04:36:58Z- handleDepth()- Depth_Keller msg: 1730867818.92 epoch sec; seqNo:252; depth 14.38 2024-11-06T04:36:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867818.93 epoch sec; seqNo:252; 4061532.40 north; 594407.90 east; 36.69 lat; -121.94 lon; 14.38 depth; utmZone: 10 2024-11-06T04:36:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867816.52 2024-11-06T04:36:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:59Z- waiting for 1730867820.52, time is 1730867818.93 2024-11-06T04:36:59Z- AHRS_M2 msg: 1730867819.33 epoch sec; seqNo:230 2024-11-06T04:36:59Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.49 psi 2024-11-06T04:36:59Z- handleDepth()- Depth_Keller msg: 1730867819.33 epoch sec; seqNo:253; depth 14.45 2024-11-06T04:36:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867819.35 epoch sec; seqNo:253; 4061532.76 north; 594406.68 east; 36.69 lat; -121.94 lon; 14.45 depth; utmZone: 10 2024-11-06T04:36:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867816.52 2024-11-06T04:36:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:59Z- waiting for 1730867820.52, time is 1730867819.35 2024-11-06T04:36:59Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:36:59Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:36:59Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:36:59Z- AHRS_M2 msg: 1730867819.71 epoch sec; seqNo:231 2024-11-06T04:36:59Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.49 psi 2024-11-06T04:36:59Z- handleDepth()- Depth_Keller msg: 1730867819.75 epoch sec; seqNo:254; depth 14.52 2024-11-06T04:36:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867819.76 epoch sec; seqNo:254; 4061533.14 north; 594406.67 east; 36.69 lat; -121.94 lon; 14.52 depth; utmZone: 10 2024-11-06T04:36:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:36:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867816.52 2024-11-06T04:36:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:36:59Z- waiting for 1730867820.52, time is 1730867819.76 2024-11-06T04:37:00Z- handleDepth()- Depth_Keller msg: 1730867820.18 epoch sec; seqNo:255; depth 14.60 2024-11-06T04:37:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867820.19 epoch sec; seqNo:255; 4061533.14 north; 594406.67 east; 36.69 lat; -121.94 lon; 14.60 depth; utmZone: 10 2024-11-06T04:37:00Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:37:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867816.52 2024-11-06T04:37:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:00Z- waiting for 1730867820.52, time is 1730867820.19 2024-11-06T04:37:00Z- AHRS_M2 msg: 1730867820.54 epoch sec; seqNo:232 2024-11-06T04:37:00Z- AHRS_M2 msg: 0.13 phi; -0.21 theta; 5.48 psi 2024-11-06T04:37:00Z- handleDepth()- Depth_Keller msg: 1730867820.55 epoch sec; seqNo:256; depth 14.66 2024-11-06T04:37:00Z- handleDVL() - DVL msg epochMillisec: 1730867820.57 2024-11-06T04:37:00Z- handleDvl() - RDI_Pathfinder msg: 1730867820.57 sec; seqNo:85; #beams:4 2024-11-06T04:37:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.17 , 66.64 , 69.91 , 90.42 2024-11-06T04:37:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.14 , -0.05 2024-11-06T04:37:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867820.60 epoch sec; seqNo:256; 4061533.52 north; 594406.67 east; 36.69 lat; -121.94 lon; 14.66 depth; utmZone: 10 2024-11-06T04:37:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867816.52 2024-11-06T04:37:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:00Z- waiting for 1730867820.52, time is 1730867820.60 2024-11-06T04:37:00Z- LcmTrn::updateTrn() >>>> pose time 1730867820.60, meas time 1730867820.57 <<<< 2024-11-06T04:37:00Z- LcmTrn::updateTrn() equating pose 1730867820.57 and meas times 1730867820.57 2024-11-06T04:37:00Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867820.57 2024-11-06T04:37:00Z- TerrainNav::motionUpdate() - currEstimate = (4061534, 594406.67, 14.66) 2024-11-06T04:37:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:37:00Z- TerrainNav:: SubcloudNIS is 0.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:37:00Z- TNavPF::Projecting Measurements 2024-11-06T04:37:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:37:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:37:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:37:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:37:00Z- TNavFilter::4 good beams. 2024-11-06T04:37:00Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:37:00Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:37:00Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:37:00Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:37:00Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:37:00Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:37:00Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:37:00Z- TerrainMapOctree: (vehN, vehE ) = (4061533.52, 594406.67). 2024-11-06T04:37:00Z- TerrainMapOctree: Min Distance = 267.22. 2024-11-06T04:37:00Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:37:00Z- Forcing Subcloud Comparison 2024-11-06T04:37:00Z- Weighting particles with subcloud comparison 2024-11-06T04:37:00Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:00Z- meanExpectedMeasDiff: 0.916003 delta_rms_squared: 1.528970 alpha: 0.987088 2024-11-06T04:37:00Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:00Z- meanExpectedMeasDiff: -3.555248 delta_rms_squared: 1.192724 alpha: 0.983508 2024-11-06T04:37:00Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:00Z- meanExpectedMeasDiff: 4.221896 delta_rms_squared: 1.576007 alpha: 0.987469 2024-11-06T04:37:00Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:00Z- meanExpectedMeasDiff: 2.081631 delta_rms_squared: 1.664379 alpha: 0.988126 2024-11-06T04:37:00Z- Subcloud NIS: 0.227608 2024-11-06T04:37:00Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:37:00Z- TNavPF:: sumWeights = 0.145880 2024-11-06T04:37:00Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:37:00Z- TNavPF::First Term of Map Covariance Matrix is 0.20 2024-11-06T04:37:00Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.02 2024-11-06T04:37:00Z- TerrainNav::Resampling particle filter... 2024-11-06T04:37:00Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730867820.57 sec, ping # 0. 2024-11-06T04:37:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:37:00Z- variances([0], [2], and psi_berg) = 11.571740 4.321044 0.000000 2024-11-06T04:37:00Z- publishEstimatesDecoder() - at 1730867820.57 sec 2024-11-06T04:37:00Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:37:00Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061549.95 594446.31 22.80 2024-11-06T04:37:00Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061549.62 594461.53 22.25 lat/lon: 36.69 -121.94 2024-11-06T04:37:00Z- LcmTrn::publishEstimatesDecoder() - COVAR: 11.57 4.32 0.63 0.00 2024-11-06T04:37:00Z- AHRS_M2 msg: 1730867820.93 epoch sec; seqNo:233 2024-11-06T04:37:00Z- AHRS_M2 msg: 0.12 phi; -0.21 theta; 5.49 psi 2024-11-06T04:37:00Z- handleDepth()- Depth_Keller msg: 1730867820.94 epoch sec; seqNo:257; depth 14.72 2024-11-06T04:37:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867820.99 epoch sec; seqNo:257; 4061533.89 north; 594405.44 east; 36.69 lat; -121.94 lon; 14.72 depth; utmZone: 10 2024-11-06T04:37:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867820.57 2024-11-06T04:37:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:01Z- waiting for 1730867824.57, time is 1730867820.99 2024-11-06T04:37:01Z- AHRS_M2 msg: 1730867821.34 epoch sec; seqNo:234 2024-11-06T04:37:01Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.49 psi 2024-11-06T04:37:01Z- handleDepth()- Depth_Keller msg: 1730867821.35 epoch sec; seqNo:258; depth 14.79 2024-11-06T04:37:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867821.36 epoch sec; seqNo:258; 4061533.89 north; 594405.44 east; 36.69 lat; -121.94 lon; 14.79 depth; utmZone: 10 2024-11-06T04:37:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867820.57 2024-11-06T04:37:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:01Z- waiting for 1730867824.57, time is 1730867821.36 2024-11-06T04:37:01Z- AHRS_M2 msg: 1730867821.74 epoch sec; seqNo:235 2024-11-06T04:37:01Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.49 psi 2024-11-06T04:37:01Z- handleDepth()- Depth_Keller msg: 1730867821.76 epoch sec; seqNo:259; depth 14.85 2024-11-06T04:37:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867821.77 epoch sec; seqNo:259; 4061534.27 north; 594405.44 east; 36.69 lat; -121.94 lon; 14.85 depth; utmZone: 10 2024-11-06T04:37:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867820.57 2024-11-06T04:37:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:01Z- waiting for 1730867824.57, time is 1730867821.77 2024-11-06T04:37:02Z- AHRS_M2 msg: 1730867822.15 epoch sec; seqNo:236 2024-11-06T04:37:02Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.48 psi 2024-11-06T04:37:02Z- handleDepth()- Depth_Keller msg: 1730867822.16 epoch sec; seqNo:260; depth 14.91 2024-11-06T04:37:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867822.18 epoch sec; seqNo:260; 4061534.27 north; 594405.44 east; 36.69 lat; -121.94 lon; 14.91 depth; utmZone: 10 2024-11-06T04:37:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867820.57 2024-11-06T04:37:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:02Z- waiting for 1730867824.57, time is 1730867822.18 2024-11-06T04:37:02Z- AHRS_M2 msg: 1730867822.55 epoch sec; seqNo:237 2024-11-06T04:37:02Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.48 psi 2024-11-06T04:37:02Z- handleDepth()- Depth_Keller msg: 1730867822.56 epoch sec; seqNo:261; depth 14.97 2024-11-06T04:37:02Z- handleDVL() - DVL msg epochMillisec: 1730867822.58 2024-11-06T04:37:02Z- handleDvl() - RDI_Pathfinder msg: 1730867822.58 sec; seqNo:86; #beams:4 2024-11-06T04:37:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.17 , 68.28 , 68.69 , 88.26 2024-11-06T04:37:02Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , -0.09 , -0.13 2024-11-06T04:37:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867822.60 epoch sec; seqNo:261; 4061534.63 north; 594404.21 east; 36.69 lat; -121.94 lon; 14.97 depth; utmZone: 10 2024-11-06T04:37:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867820.57 2024-11-06T04:37:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:02Z- waiting for 1730867824.57, time is 1730867822.60 2024-11-06T04:37:02Z- AHRS_M2 msg: 1730867822.95 epoch sec; seqNo:238 2024-11-06T04:37:02Z- AHRS_M2 msg: 0.11 phi; -0.19 theta; 5.48 psi 2024-11-06T04:37:02Z- handleDepth()- Depth_Keller msg: 1730867822.96 epoch sec; seqNo:262; depth 15.05 2024-11-06T04:37:02Z- handleDVL() - DVL msg epochMillisec: 1730867822.96 2024-11-06T04:37:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:37:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:37:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:37:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:37:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:37:02Z- handleDvl() - RDI_Pathfinder msg: 1730867822.96 sec; seqNo:87; #beams:0 2024-11-06T04:37:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:37:02Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , -0.09 , -0.13 2024-11-06T04:37:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867822.98 epoch sec; seqNo:262; 4061535.01 north; 594404.21 east; 36.69 lat; -121.94 lon; 15.05 depth; utmZone: 10 2024-11-06T04:37:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867820.57 2024-11-06T04:37:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:03Z- waiting for 1730867824.57, time is 1730867822.98 2024-11-06T04:37:03Z- AHRS_M2 msg: 1730867823.36 epoch sec; seqNo:239 2024-11-06T04:37:03Z- AHRS_M2 msg: 0.11 phi; -0.19 theta; 5.48 psi 2024-11-06T04:37:03Z- handleDepth()- Depth_Keller msg: 1730867823.36 epoch sec; seqNo:263; depth 15.12 2024-11-06T04:37:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867823.38 epoch sec; seqNo:263; 4061535.39 north; 594404.21 east; 36.69 lat; -121.94 lon; 15.12 depth; utmZone: 10 2024-11-06T04:37:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867820.57 2024-11-06T04:37:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:03Z- waiting for 1730867824.57, time is 1730867823.38 2024-11-06T04:37:03Z- AHRS_M2 msg: 1730867823.76 epoch sec; seqNo:240 2024-11-06T04:37:03Z- AHRS_M2 msg: 0.11 phi; -0.19 theta; 5.48 psi 2024-11-06T04:37:03Z- handleDepth()- Depth_Keller msg: 1730867823.77 epoch sec; seqNo:264; depth 15.18 2024-11-06T04:37:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867823.78 epoch sec; seqNo:264; 4061535.39 north; 594404.21 east; 36.69 lat; -121.94 lon; 15.18 depth; utmZone: 10 2024-11-06T04:37:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867820.57 2024-11-06T04:37:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:03Z- waiting for 1730867824.57, time is 1730867823.78 2024-11-06T04:37:04Z- AHRS_M2 msg: 1730867824.16 epoch sec; seqNo:241 2024-11-06T04:37:04Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.48 psi 2024-11-06T04:37:04Z- handleDepth()- Depth_Keller msg: 1730867824.17 epoch sec; seqNo:265; depth 15.27 2024-11-06T04:37:04Z- handleDVL() - DVL msg epochMillisec: 1730867824.18 2024-11-06T04:37:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:37:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:37:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:37:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:37:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:37:04Z- handleDvl() - RDI_Pathfinder msg: 1730867824.18 sec; seqNo:88; #beams:0 2024-11-06T04:37:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:37:04Z- handleDvl() - RDI_Pathfinder msg: velocities 1.09 , -0.09 , -0.13 2024-11-06T04:37:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867824.19 epoch sec; seqNo:265; 4061535.75 north; 594402.98 east; 36.69 lat; -121.94 lon; 15.27 depth; utmZone: 10 2024-11-06T04:37:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867820.57 2024-11-06T04:37:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:04Z- waiting for 1730867824.57, time is 1730867824.19 2024-11-06T04:37:04Z- AHRS_M2 msg: 1730867824.57 epoch sec; seqNo:242 2024-11-06T04:37:04Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.48 psi 2024-11-06T04:37:04Z- handleDepth()- Depth_Keller msg: 1730867824.58 epoch sec; seqNo:266; depth 15.34 2024-11-06T04:37:04Z- handleDVL() - DVL msg epochMillisec: 1730867824.60 2024-11-06T04:37:04Z- handleDvl() - RDI_Pathfinder msg: 1730867824.60 sec; seqNo:89; #beams:4 2024-11-06T04:37:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.09 , 67.87 , 69.10 , 89.33 2024-11-06T04:37:04Z- handleDvl() - RDI_Pathfinder msg: velocities 1.23 , -0.14 , 0.09 2024-11-06T04:37:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867824.62 epoch sec; seqNo:266; 4061536.13 north; 594402.98 east; 36.69 lat; -121.94 lon; 15.34 depth; utmZone: 10 2024-11-06T04:37:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867820.57 2024-11-06T04:37:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:04Z- waiting for 1730867824.57, time is 1730867824.62 2024-11-06T04:37:04Z- LcmTrn::updateTrn() >>>> pose time 1730867824.62, meas time 1730867824.60 <<<< 2024-11-06T04:37:04Z- LcmTrn::updateTrn() equating pose 1730867824.60 and meas times 1730867824.60 2024-11-06T04:37:04Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867824.60 2024-11-06T04:37:04Z- TerrainNav::motionUpdate() - currEstimate = (4061536, 594402.98, 15.34) 2024-11-06T04:37:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:37:04Z- TerrainNav:: SubcloudNIS is 0.23, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:37:04Z- TNavPF::Projecting Measurements 2024-11-06T04:37:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:37:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:37:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:37:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:37:04Z- TNavFilter::4 good beams. 2024-11-06T04:37:04Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:37:04Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:37:04Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:37:04Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:37:04Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:37:04Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:37:04Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:37:04Z- TerrainMapOctree: (vehN, vehE ) = (4061536.13, 594402.98). 2024-11-06T04:37:04Z- TerrainMapOctree: Min Distance = 262.95. 2024-11-06T04:37:04Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:37:04Z- Forcing Subcloud Comparison 2024-11-06T04:37:04Z- Weighting particles with subcloud comparison 2024-11-06T04:37:04Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:04Z- meanExpectedMeasDiff: 1.222042 delta_rms_squared: 1.503979 alpha: 0.986876 2024-11-06T04:37:04Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:04Z- meanExpectedMeasDiff: -1.717881 delta_rms_squared: 1.123810 alpha: 0.982515 2024-11-06T04:37:04Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:04Z- meanExpectedMeasDiff: 5.203530 delta_rms_squared: 1.247433 alpha: 0.984220 2024-11-06T04:37:04Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:04Z- meanExpectedMeasDiff: 2.988469 delta_rms_squared: 1.528719 alpha: 0.987086 2024-11-06T04:37:04Z- Subcloud NIS: 0.256951 2024-11-06T04:37:04Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:37:04Z- TNavPF:: sumWeights = 0.144608 2024-11-06T04:37:04Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:37:04Z- TNavPF::First Term of Map Covariance Matrix is 0.19 2024-11-06T04:37:04Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.01 2024-11-06T04:37:04Z- TerrainNav::Resampling particle filter... 2024-11-06T04:37:04Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730867824.60 sec, ping # 0. 2024-11-06T04:37:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:37:04Z- variances([0], [2], and psi_berg) = 16.617447 6.499385 0.000000 2024-11-06T04:37:04Z- publishEstimatesDecoder() - at 1730867824.60 sec 2024-11-06T04:37:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:37:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061552.13 594443.17 21.68 2024-11-06T04:37:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061551.59 594458.05 22.50 lat/lon: 36.69 -121.94 2024-11-06T04:37:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 16.62 6.50 0.62 0.00 2024-11-06T04:37:04Z- AHRS_M2 msg: 1730867824.97 epoch sec; seqNo:243 2024-11-06T04:37:04Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.48 psi 2024-11-06T04:37:04Z- handleDepth()- Depth_Keller msg: 1730867824.98 epoch sec; seqNo:267; depth 15.41 2024-11-06T04:37:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867825.02 epoch sec; seqNo:267; 4061536.51 north; 594402.97 east; 36.69 lat; -121.94 lon; 15.41 depth; utmZone: 10 2024-11-06T04:37:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867824.60 2024-11-06T04:37:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:05Z- waiting for 1730867828.60, time is 1730867825.02 2024-11-06T04:37:05Z- AHRS_M2 msg: 1730867825.38 epoch sec; seqNo:244 2024-11-06T04:37:05Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.47 psi 2024-11-06T04:37:05Z- handleDepth()- Depth_Keller msg: 1730867825.39 epoch sec; seqNo:268; depth 15.48 2024-11-06T04:37:05Z- handleDVL() - DVL msg epochMillisec: 1730867825.40 2024-11-06T04:37:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:37:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:37:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:37:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:37:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:37:05Z- handleDvl() - RDI_Pathfinder msg: 1730867825.40 sec; seqNo:90; #beams:0 2024-11-06T04:37:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:37:05Z- handleDvl() - RDI_Pathfinder msg: velocities 1.23 , -0.14 , 0.09 2024-11-06T04:37:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867825.42 epoch sec; seqNo:268; 4061536.88 north; 594401.75 east; 36.69 lat; -121.94 lon; 15.48 depth; utmZone: 10 2024-11-06T04:37:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867824.60 2024-11-06T04:37:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:05Z- waiting for 1730867828.60, time is 1730867825.42 2024-11-06T04:37:05Z- AHRS_M2 msg: 1730867825.79 epoch sec; seqNo:245 2024-11-06T04:37:05Z- AHRS_M2 msg: 0.13 phi; -0.20 theta; 5.47 psi 2024-11-06T04:37:05Z- handleDepth()- Depth_Keller msg: 1730867825.79 epoch sec; seqNo:269; depth 15.55 2024-11-06T04:37:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867825.80 epoch sec; seqNo:269; 4061536.88 north; 594401.75 east; 36.69 lat; -121.94 lon; 15.55 depth; utmZone: 10 2024-11-06T04:37:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867824.60 2024-11-06T04:37:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:05Z- waiting for 1730867828.60, time is 1730867825.80 2024-11-06T04:37:06Z- AHRS_M2 msg: 1730867826.19 epoch sec; seqNo:246 2024-11-06T04:37:06Z- AHRS_M2 msg: 0.13 phi; -0.20 theta; 5.47 psi 2024-11-06T04:37:06Z- handleDepth()- Depth_Keller msg: 1730867826.20 epoch sec; seqNo:270; depth 15.64 2024-11-06T04:37:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867826.22 epoch sec; seqNo:270; 4061537.26 north; 594401.75 east; 36.69 lat; -121.94 lon; 15.64 depth; utmZone: 10 2024-11-06T04:37:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867824.60 2024-11-06T04:37:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:06Z- waiting for 1730867828.60, time is 1730867826.22 2024-11-06T04:37:06Z- AHRS_M2 msg: 1730867826.59 epoch sec; seqNo:247 2024-11-06T04:37:06Z- AHRS_M2 msg: 0.13 phi; -0.21 theta; 5.47 psi 2024-11-06T04:37:06Z- handleDepth()- Depth_Keller msg: 1730867826.60 epoch sec; seqNo:271; depth 15.72 2024-11-06T04:37:06Z- handleDVL() - DVL msg epochMillisec: 1730867826.63 2024-11-06T04:37:06Z- handleDvl() - RDI_Pathfinder msg: 1730867826.63 sec; seqNo:91; #beams:4 2024-11-06T04:37:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.09 , 68.28 , 69.10 , 87.17 2024-11-06T04:37:06Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , -0.07 , 0.18 2024-11-06T04:37:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867826.64 epoch sec; seqNo:271; 4061537.62 north; 594400.52 east; 36.69 lat; -121.94 lon; 15.72 depth; utmZone: 10 2024-11-06T04:37:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867824.60 2024-11-06T04:37:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:06Z- waiting for 1730867828.60, time is 1730867826.64 2024-11-06T04:37:07Z- AHRS_M2 msg: 1730867827.00 epoch sec; seqNo:248 2024-11-06T04:37:07Z- AHRS_M2 msg: 0.13 phi; -0.21 theta; 5.47 psi 2024-11-06T04:37:07Z- handleDepth()- Depth_Keller msg: 1730867827.00 epoch sec; seqNo:272; depth 15.78 2024-11-06T04:37:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867827.02 epoch sec; seqNo:272; 4061537.62 north; 594400.52 east; 36.69 lat; -121.94 lon; 15.78 depth; utmZone: 10 2024-11-06T04:37:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867824.60 2024-11-06T04:37:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:07Z- waiting for 1730867828.60, time is 1730867827.02 2024-11-06T04:37:07Z- handleDepth()- Depth_Keller msg: 1730867827.42 epoch sec; seqNo:273; depth 15.85 2024-11-06T04:37:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867827.43 epoch sec; seqNo:273; 4061538.00 north; 594400.52 east; 36.69 lat; -121.94 lon; 15.85 depth; utmZone: 10 2024-11-06T04:37:07Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:37:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867824.60 2024-11-06T04:37:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:07Z- waiting for 1730867828.60, time is 1730867827.43 2024-11-06T04:37:07Z- AHRS_M2 msg: 1730867827.80 epoch sec; seqNo:249 2024-11-06T04:37:07Z- AHRS_M2 msg: 0.13 phi; -0.21 theta; 5.47 psi 2024-11-06T04:37:07Z- handleDepth()- Depth_Keller msg: 1730867827.82 epoch sec; seqNo:274; depth 15.92 2024-11-06T04:37:07Z- handleDVL() - DVL msg epochMillisec: 1730867827.82 2024-11-06T04:37:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:37:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:37:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:37:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:37:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:37:07Z- handleDvl() - RDI_Pathfinder msg: 1730867827.82 sec; seqNo:92; #beams:0 2024-11-06T04:37:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:37:07Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , -0.07 , 0.18 2024-11-06T04:37:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867827.83 epoch sec; seqNo:274; 4061538.38 north; 594400.51 east; 36.69 lat; -121.94 lon; 15.92 depth; utmZone: 10 2024-11-06T04:37:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867824.60 2024-11-06T04:37:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:07Z- waiting for 1730867828.60, time is 1730867827.83 2024-11-06T04:37:08Z- AHRS_M2 msg: 1730867828.21 epoch sec; seqNo:250 2024-11-06T04:37:08Z- AHRS_M2 msg: 0.12 phi; -0.21 theta; 5.47 psi 2024-11-06T04:37:08Z- handleDepth()- Depth_Keller msg: 1730867828.22 epoch sec; seqNo:275; depth 15.99 2024-11-06T04:37:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867828.23 epoch sec; seqNo:275; 4061538.37 north; 594399.29 east; 36.69 lat; -121.94 lon; 15.99 depth; utmZone: 10 2024-11-06T04:37:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867824.60 2024-11-06T04:37:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:08Z- waiting for 1730867828.60, time is 1730867828.23 2024-11-06T04:37:08Z- AHRS_M2 msg: 1730867828.61 epoch sec; seqNo:251 2024-11-06T04:37:08Z- AHRS_M2 msg: 0.12 phi; -0.21 theta; 5.47 psi 2024-11-06T04:37:08Z- handleDepth()- Depth_Keller msg: 1730867828.62 epoch sec; seqNo:276; depth 16.07 2024-11-06T04:37:08Z- handleDVL() - DVL msg epochMillisec: 1730867828.64 2024-11-06T04:37:08Z- handleDvl() - RDI_Pathfinder msg: 1730867828.64 sec; seqNo:93; #beams:4 2024-11-06T04:37:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.09 , 68.28 , 68.69 , 87.17 2024-11-06T04:37:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.06 , 0.13 2024-11-06T04:37:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867828.66 epoch sec; seqNo:276; 4061538.74 north; 594399.29 east; 36.69 lat; -121.94 lon; 16.07 depth; utmZone: 10 2024-11-06T04:37:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867824.60 2024-11-06T04:37:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:08Z- waiting for 1730867828.60, time is 1730867828.66 2024-11-06T04:37:08Z- LcmTrn::updateTrn() >>>> pose time 1730867828.66, meas time 1730867828.64 <<<< 2024-11-06T04:37:08Z- LcmTrn::updateTrn() equating pose 1730867828.64 and meas times 1730867828.64 2024-11-06T04:37:08Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867828.64 2024-11-06T04:37:08Z- TerrainNav::motionUpdate() - currEstimate = (4061539, 594399.29, 16.07) 2024-11-06T04:37:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:37:08Z- TerrainNav:: SubcloudNIS is 0.26, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:37:08Z- TNavPF::Projecting Measurements 2024-11-06T04:37:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:37:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:37:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:37:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:37:08Z- TNavFilter::4 good beams. 2024-11-06T04:37:08Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:37:08Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:37:08Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:37:08Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:37:08Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:37:08Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:37:08Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:37:08Z- TerrainMapOctree: (vehN, vehE ) = (4061538.74, 594399.29). 2024-11-06T04:37:08Z- TerrainMapOctree: Min Distance = 258.68. 2024-11-06T04:37:08Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:37:08Z- Forcing Subcloud Comparison 2024-11-06T04:37:08Z- Weighting particles with subcloud comparison 2024-11-06T04:37:08Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:08Z- meanExpectedMeasDiff: 1.085704 delta_rms_squared: 1.399403 alpha: 0.985910 2024-11-06T04:37:08Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:08Z- meanExpectedMeasDiff: -1.981124 delta_rms_squared: 1.311393 alpha: 0.984978 2024-11-06T04:37:08Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:08Z- meanExpectedMeasDiff: 5.747264 delta_rms_squared: 0.983850 alpha: 0.980077 2024-11-06T04:37:08Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:08Z- meanExpectedMeasDiff: -0.486685 delta_rms_squared: 1.566239 alpha: 0.987392 2024-11-06T04:37:08Z- Subcloud NIS: 0.273704 2024-11-06T04:37:08Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:37:08Z- TNavPF:: sumWeights = 0.120050 2024-11-06T04:37:08Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:37:08Z- TNavPF::First Term of Map Covariance Matrix is 0.18 2024-11-06T04:37:08Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T04:37:08Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730867828.64 sec, ping # 0. 2024-11-06T04:37:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:37:08Z- variances([0], [2], and psi_berg) = 38.201331 20.027609 0.000000 2024-11-06T04:37:08Z- publishEstimatesDecoder() - at 1730867828.64 sec 2024-11-06T04:37:08Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:37:08Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061522.23 594426.54 24.12 2024-11-06T04:37:08Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061553.23 594454.02 23.01 lat/lon: 36.69 -121.94 2024-11-06T04:37:08Z- LcmTrn::publishEstimatesDecoder() - COVAR: 38.20 20.03 0.70 0.00 2024-11-06T04:37:09Z- AHRS_M2 msg: 1730867829.02 epoch sec; seqNo:252 2024-11-06T04:37:09Z- AHRS_M2 msg: 0.12 phi; -0.21 theta; 5.47 psi 2024-11-06T04:37:09Z- handleDepth()- Depth_Keller msg: 1730867829.03 epoch sec; seqNo:277; depth 16.14 2024-11-06T04:37:09Z- handleDVL() - DVL msg epochMillisec: 1730867829.03 2024-11-06T04:37:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:37:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:37:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:37:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:37:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:37:09Z- handleDvl() - RDI_Pathfinder msg: 1730867829.03 sec; seqNo:94; #beams:0 2024-11-06T04:37:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:37:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.06 , 0.13 2024-11-06T04:37:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867829.05 epoch sec; seqNo:277; 4061539.12 north; 594399.28 east; 36.69 lat; -121.94 lon; 16.14 depth; utmZone: 10 2024-11-06T04:37:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867828.64 2024-11-06T04:37:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:09Z- waiting for 1730867832.64, time is 1730867829.05 2024-11-06T04:37:09Z- AHRS_M2 msg: 1730867829.42 epoch sec; seqNo:253 2024-11-06T04:37:09Z- AHRS_M2 msg: 0.12 phi; -0.22 theta; 5.47 psi 2024-11-06T04:37:09Z- handleDepth()- Depth_Keller msg: 1730867829.43 epoch sec; seqNo:278; depth 16.21 2024-11-06T04:37:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867829.44 epoch sec; seqNo:278; 4061539.12 north; 594399.28 east; 36.69 lat; -121.94 lon; 16.21 depth; utmZone: 10 2024-11-06T04:37:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867828.64 2024-11-06T04:37:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:09Z- waiting for 1730867832.64, time is 1730867829.44 2024-11-06T04:37:09Z- AHRS_M2 msg: 1730867829.83 epoch sec; seqNo:254 2024-11-06T04:37:09Z- AHRS_M2 msg: 0.11 phi; -0.22 theta; 5.48 psi 2024-11-06T04:37:09Z- handleDepth()- Depth_Keller msg: 1730867829.84 epoch sec; seqNo:279; depth 16.29 2024-11-06T04:37:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867829.86 epoch sec; seqNo:279; 4061539.49 north; 594398.06 east; 36.69 lat; -121.94 lon; 16.29 depth; utmZone: 10 2024-11-06T04:37:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867828.64 2024-11-06T04:37:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:09Z- waiting for 1730867832.64, time is 1730867829.86 2024-11-06T04:37:10Z- AHRS_M2 msg: 1730867830.23 epoch sec; seqNo:255 2024-11-06T04:37:10Z- AHRS_M2 msg: 0.12 phi; -0.22 theta; 5.48 psi 2024-11-06T04:37:10Z- handleDepth()- Depth_Keller msg: 1730867830.24 epoch sec; seqNo:280; depth 16.36 2024-11-06T04:37:10Z- handleDVL() - DVL msg epochMillisec: 1730867830.24 2024-11-06T04:37:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:37:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:37:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:37:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:37:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:37:10Z- handleDvl() - RDI_Pathfinder msg: 1730867830.24 sec; seqNo:95; #beams:0 2024-11-06T04:37:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:37:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.06 , 0.13 2024-11-06T04:37:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867830.29 epoch sec; seqNo:280; 4061539.49 north; 594398.06 east; 36.69 lat; -121.94 lon; 16.36 depth; utmZone: 10 2024-11-06T04:37:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867828.64 2024-11-06T04:37:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:10Z- waiting for 1730867832.64, time is 1730867830.29 2024-11-06T04:37:10Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:37:10Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:37:10Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:37:10Z- AHRS_M2 msg: 1730867830.62 epoch sec; seqNo:256 2024-11-06T04:37:10Z- AHRS_M2 msg: 0.12 phi; -0.22 theta; 5.48 psi 2024-11-06T04:37:10Z- handleDepth()- Depth_Keller msg: 1730867830.65 epoch sec; seqNo:281; depth 16.41 2024-11-06T04:37:10Z- handleDVL() - DVL msg epochMillisec: 1730867830.68 2024-11-06T04:37:10Z- handleDvl() - RDI_Pathfinder msg: 1730867830.68 sec; seqNo:96; #beams:4 2024-11-06T04:37:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.09 , 67.47 , 67.88 , 88.26 2024-11-06T04:37:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.07 , 0.05 2024-11-06T04:37:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867830.70 epoch sec; seqNo:281; 4061539.87 north; 594398.05 east; 36.69 lat; -121.94 lon; 16.41 depth; utmZone: 10 2024-11-06T04:37:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867828.64 2024-11-06T04:37:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:10Z- waiting for 1730867832.64, time is 1730867830.70 2024-11-06T04:37:11Z- AHRS_M2 msg: 1730867831.04 epoch sec; seqNo:257 2024-11-06T04:37:11Z- AHRS_M2 msg: 0.12 phi; -0.22 theta; 5.48 psi 2024-11-06T04:37:11Z- handleDepth()- Depth_Keller msg: 1730867831.04 epoch sec; seqNo:282; depth 16.51 2024-11-06T04:37:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867831.06 epoch sec; seqNo:282; 4061539.87 north; 594398.05 east; 36.69 lat; -121.94 lon; 16.51 depth; utmZone: 10 2024-11-06T04:37:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867828.64 2024-11-06T04:37:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:11Z- waiting for 1730867832.64, time is 1730867831.06 2024-11-06T04:37:11Z- AHRS_M2 msg: 1730867831.44 epoch sec; seqNo:258 2024-11-06T04:37:11Z- AHRS_M2 msg: 0.12 phi; -0.22 theta; 5.49 psi 2024-11-06T04:37:11Z- handleDepth()- Depth_Keller msg: 1730867831.45 epoch sec; seqNo:283; depth 16.56 2024-11-06T04:37:11Z- handleDVL() - DVL msg epochMillisec: 1730867831.45 2024-11-06T04:37:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:37:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:37:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:37:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:37:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:37:11Z- handleDvl() - RDI_Pathfinder msg: 1730867831.45 sec; seqNo:97; #beams:0 2024-11-06T04:37:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:37:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.07 , 0.05 2024-11-06T04:37:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867831.46 epoch sec; seqNo:283; 4061540.25 north; 594398.05 east; 36.69 lat; -121.94 lon; 16.56 depth; utmZone: 10 2024-11-06T04:37:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867828.64 2024-11-06T04:37:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:11Z- waiting for 1730867832.64, time is 1730867831.46 2024-11-06T04:37:11Z- AHRS_M2 msg: 1730867831.85 epoch sec; seqNo:259 2024-11-06T04:37:11Z- AHRS_M2 msg: 0.12 phi; -0.22 theta; 5.49 psi 2024-11-06T04:37:11Z- handleDepth()- Depth_Keller msg: 1730867831.86 epoch sec; seqNo:284; depth 16.64 2024-11-06T04:37:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867831.87 epoch sec; seqNo:284; 4061540.61 north; 594396.83 east; 36.69 lat; -121.94 lon; 16.64 depth; utmZone: 10 2024-11-06T04:37:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867828.64 2024-11-06T04:37:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:12Z- waiting for 1730867832.64, time is 1730867831.87 2024-11-06T04:37:12Z- AHRS_M2 msg: 1730867832.25 epoch sec; seqNo:260 2024-11-06T04:37:12Z- AHRS_M2 msg: 0.13 phi; -0.22 theta; 5.49 psi 2024-11-06T04:37:12Z- handleDepth()- Depth_Keller msg: 1730867832.25 epoch sec; seqNo:285; depth 16.70 2024-11-06T04:37:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867832.26 epoch sec; seqNo:285; 4061540.61 north; 594396.83 east; 36.69 lat; -121.94 lon; 16.70 depth; utmZone: 10 2024-11-06T04:37:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867828.64 2024-11-06T04:37:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:12Z- waiting for 1730867832.64, time is 1730867832.26 2024-11-06T04:37:12Z- handleDepth()- Depth_Keller msg: 1730867832.67 epoch sec; seqNo:286; depth 16.78 2024-11-06T04:37:12Z- handleDVL() - DVL msg epochMillisec: 1730867832.70 2024-11-06T04:37:12Z- handleDvl() - RDI_Pathfinder msg: 1730867832.70 sec; seqNo:98; #beams:4 2024-11-06T04:37:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 87.17 , 66.64 , 67.87 , 89.33 2024-11-06T04:37:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.03 , -0.05 2024-11-06T04:37:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867832.71 epoch sec; seqNo:286; 4061540.99 north; 594396.82 east; 36.69 lat; -121.94 lon; 16.78 depth; utmZone: 10 2024-11-06T04:37:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867828.64 2024-11-06T04:37:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:12Z- waiting for 1730867832.64, time is 1730867832.71 2024-11-06T04:37:12Z- LcmTrn::updateTrn() >>>> pose time 1730867832.71, meas time 1730867832.70 <<<< 2024-11-06T04:37:12Z- LcmTrn::updateTrn() equating pose 1730867832.70 and meas times 1730867832.70 2024-11-06T04:37:12Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867832.70 2024-11-06T04:37:12Z- TerrainNav::motionUpdate() - currEstimate = (4061541, 594396.82, 16.78) 2024-11-06T04:37:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:37:12Z- TerrainNav:: SubcloudNIS is 0.27, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:37:12Z- TNavPF::Projecting Measurements 2024-11-06T04:37:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:37:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:37:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:37:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:37:12Z- TNavFilter::4 good beams. 2024-11-06T04:37:12Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:37:12Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:37:12Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:37:12Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:37:12Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:37:12Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:37:12Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:37:12Z- TerrainMapOctree: (vehN, vehE ) = (4061540.99, 594396.82). 2024-11-06T04:37:12Z- TerrainMapOctree: Min Distance = 255.42. 2024-11-06T04:37:12Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:37:12Z- Forcing Subcloud Comparison 2024-11-06T04:37:12Z- Weighting particles with subcloud comparison 2024-11-06T04:37:12Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:12Z- meanExpectedMeasDiff: 2.201702 delta_rms_squared: 1.469390 alpha: 0.986572 2024-11-06T04:37:12Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:12Z- meanExpectedMeasDiff: -3.245153 delta_rms_squared: 1.321192 alpha: 0.985088 2024-11-06T04:37:12Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:12Z- meanExpectedMeasDiff: 5.551265 delta_rms_squared: 1.345431 alpha: 0.985353 2024-11-06T04:37:12Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:12Z- meanExpectedMeasDiff: 1.260468 delta_rms_squared: 1.675682 alpha: 0.988205 2024-11-06T04:37:12Z- Subcloud NIS: 0.305021 2024-11-06T04:37:12Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:37:12Z- TNavPF:: sumWeights = 0.077993 2024-11-06T04:37:12Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:37:13Z- TNavPF::First Term of Map Covariance Matrix is 0.34 2024-11-06T04:37:13Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.01 2024-11-06T04:37:13Z- TerrainNav::Resampling particle filter... 2024-11-06T04:37:13Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730867832.70 sec, ping # 0. 2024-11-06T04:37:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:37:13Z- variances([0], [2], and psi_berg) = 97.282635 66.619069 0.000000 2024-11-06T04:37:13Z- publishEstimatesDecoder() - at 1730867832.70 sec 2024-11-06T04:37:13Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:37:13Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061557.07 594437.00 21.49 2024-11-06T04:37:13Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061552.89 594449.97 23.55 lat/lon: 36.69 -121.94 2024-11-06T04:37:13Z- LcmTrn::publishEstimatesDecoder() - COVAR: 97.28 66.62 1.04 0.00 2024-11-06T04:37:13Z- handleDepth()- Depth_Keller msg: 1730867833.09 epoch sec; seqNo:287; depth 16.86 2024-11-06T04:37:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867833.12 epoch sec; seqNo:287; 4061540.99 north; 594396.82 east; 36.69 lat; -121.94 lon; 16.86 depth; utmZone: 10 2024-11-06T04:37:13Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:37:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867832.70 2024-11-06T04:37:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:13Z- waiting for 1730867836.70, time is 1730867833.12 2024-11-06T04:37:13Z- AHRS_M2 msg: 1730867833.46 epoch sec; seqNo:261 2024-11-06T04:37:13Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.50 psi 2024-11-06T04:37:13Z- handleDepth()- Depth_Keller msg: 1730867833.47 epoch sec; seqNo:288; depth 16.93 2024-11-06T04:37:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867833.48 epoch sec; seqNo:288; 4061541.37 north; 594396.82 east; 36.69 lat; -121.94 lon; 16.93 depth; utmZone: 10 2024-11-06T04:37:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867832.70 2024-11-06T04:37:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:13Z- waiting for 1730867836.70, time is 1730867833.48 2024-11-06T04:37:13Z- AHRS_M2 msg: 1730867833.86 epoch sec; seqNo:262 2024-11-06T04:37:13Z- AHRS_M2 msg: 0.12 phi; -0.23 theta; 5.51 psi 2024-11-06T04:37:13Z- handleDepth()- Depth_Keller msg: 1730867833.88 epoch sec; seqNo:289; depth 16.98 2024-11-06T04:37:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867833.88 epoch sec; seqNo:289; 4061541.73 north; 594395.59 east; 36.69 lat; -121.94 lon; 16.98 depth; utmZone: 10 2024-11-06T04:37:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867832.70 2024-11-06T04:37:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:14Z- waiting for 1730867836.70, time is 1730867833.88 2024-11-06T04:37:14Z- AHRS_M2 msg: 1730867834.27 epoch sec; seqNo:263 2024-11-06T04:37:14Z- AHRS_M2 msg: 0.12 phi; -0.23 theta; 5.51 psi 2024-11-06T04:37:14Z- handleDepth()- Depth_Keller msg: 1730867834.28 epoch sec; seqNo:290; depth 17.06 2024-11-06T04:37:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867834.29 epoch sec; seqNo:290; 4061541.73 north; 594395.59 east; 36.69 lat; -121.94 lon; 17.06 depth; utmZone: 10 2024-11-06T04:37:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867832.70 2024-11-06T04:37:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:14Z- waiting for 1730867836.70, time is 1730867834.29 2024-11-06T04:37:14Z- AHRS_M2 msg: 1730867834.68 epoch sec; seqNo:264 2024-11-06T04:37:14Z- AHRS_M2 msg: 0.11 phi; -0.23 theta; 5.51 psi 2024-11-06T04:37:14Z- handleDepth()- Depth_Keller msg: 1730867834.69 epoch sec; seqNo:291; depth 17.13 2024-11-06T04:37:14Z- handleDVL() - DVL msg epochMillisec: 1730867834.71 2024-11-06T04:37:14Z- handleDvl() - RDI_Pathfinder msg: 1730867834.71 sec; seqNo:99; #beams:4 2024-11-06T04:37:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 86.09 , 67.46 , 66.64 , 90.42 2024-11-06T04:37:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.03 , -0.19 2024-11-06T04:37:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867834.73 epoch sec; seqNo:291; 4061542.11 north; 594395.59 east; 36.69 lat; -121.94 lon; 17.13 depth; utmZone: 10 2024-11-06T04:37:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867832.70 2024-11-06T04:37:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:14Z- waiting for 1730867836.70, time is 1730867834.73 2024-11-06T04:37:15Z- AHRS_M2 msg: 1730867835.08 epoch sec; seqNo:265 2024-11-06T04:37:15Z- AHRS_M2 msg: 0.11 phi; -0.23 theta; 5.51 psi 2024-11-06T04:37:15Z- handleDepth()- Depth_Keller msg: 1730867835.08 epoch sec; seqNo:292; depth 17.22 2024-11-06T04:37:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867835.10 epoch sec; seqNo:292; 4061542.49 north; 594395.58 east; 36.69 lat; -121.94 lon; 17.22 depth; utmZone: 10 2024-11-06T04:37:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867832.70 2024-11-06T04:37:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:15Z- waiting for 1730867836.70, time is 1730867835.10 2024-11-06T04:37:15Z- handleDepth()- Depth_Keller msg: 1730867835.50 epoch sec; seqNo:293; depth 17.27 2024-11-06T04:37:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867835.51 epoch sec; seqNo:293; 4061542.87 north; 594395.58 east; 36.69 lat; -121.94 lon; 17.27 depth; utmZone: 10 2024-11-06T04:37:15Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:37:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867832.70 2024-11-06T04:37:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:15Z- waiting for 1730867836.70, time is 1730867835.51 2024-11-06T04:37:15Z- AHRS_M2 msg: 1730867835.88 epoch sec; seqNo:266 2024-11-06T04:37:15Z- AHRS_M2 msg: 0.10 phi; -0.23 theta; 5.50 psi 2024-11-06T04:37:15Z- handleDepth()- Depth_Keller msg: 1730867835.89 epoch sec; seqNo:294; depth 17.37 2024-11-06T04:37:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867835.91 epoch sec; seqNo:294; 4061542.86 north; 594394.36 east; 36.69 lat; -121.94 lon; 17.37 depth; utmZone: 10 2024-11-06T04:37:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867832.70 2024-11-06T04:37:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:16Z- waiting for 1730867836.70, time is 1730867835.91 2024-11-06T04:37:16Z- AHRS_M2 msg: 1730867836.29 epoch sec; seqNo:267 2024-11-06T04:37:16Z- AHRS_M2 msg: 0.09 phi; -0.23 theta; 5.50 psi 2024-11-06T04:37:16Z- handleDepth()- Depth_Keller msg: 1730867836.29 epoch sec; seqNo:295; depth 17.44 2024-11-06T04:37:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867836.30 epoch sec; seqNo:295; 4061543.24 north; 594394.36 east; 36.69 lat; -121.94 lon; 17.44 depth; utmZone: 10 2024-11-06T04:37:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867832.70 2024-11-06T04:37:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:16Z- waiting for 1730867836.70, time is 1730867836.30 2024-11-06T04:37:16Z- AHRS_M2 msg: 1730867836.69 epoch sec; seqNo:268 2024-11-06T04:37:16Z- AHRS_M2 msg: 0.09 phi; -0.22 theta; 5.50 psi 2024-11-06T04:37:16Z- handleDepth()- Depth_Keller msg: 1730867836.70 epoch sec; seqNo:296; depth 17.52 2024-11-06T04:37:16Z- handleDVL() - DVL msg epochMillisec: 1730867836.72 2024-11-06T04:37:16Z- handleDvl() - RDI_Pathfinder msg: 1730867836.72 sec; seqNo:100; #beams:4 2024-11-06T04:37:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.50 , 67.87 , 65.42 , 87.17 2024-11-06T04:37:16Z- handleDvl() - RDI_Pathfinder msg: velocities 1.23 , -0.11 , -0.10 2024-11-06T04:37:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867836.74 epoch sec; seqNo:296; 4061543.62 north; 594394.35 east; 36.69 lat; -121.94 lon; 17.52 depth; utmZone: 10 2024-11-06T04:37:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867832.70 2024-11-06T04:37:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:16Z- waiting for 1730867836.70, time is 1730867836.74 2024-11-06T04:37:16Z- LcmTrn::updateTrn() >>>> pose time 1730867836.74, meas time 1730867836.72 <<<< 2024-11-06T04:37:16Z- LcmTrn::updateTrn() equating pose 1730867836.72 and meas times 1730867836.72 2024-11-06T04:37:16Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867836.72 2024-11-06T04:37:16Z- TerrainNav::motionUpdate() - currEstimate = (4061544, 594394.35, 17.52) 2024-11-06T04:37:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:37:16Z- TerrainNav:: SubcloudNIS is 0.31, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:37:16Z- TNavPF::Projecting Measurements 2024-11-06T04:37:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:37:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:37:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:37:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:37:16Z- TNavFilter::4 good beams. 2024-11-06T04:37:16Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:37:16Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:37:16Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:37:16Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:37:16Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:37:16Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:37:16Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:37:16Z- TerrainMapOctree: (vehN, vehE ) = (4061543.62, 594394.35). 2024-11-06T04:37:16Z- TerrainMapOctree: Min Distance = 251.86. 2024-11-06T04:37:16Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:37:16Z- Forcing Subcloud Comparison 2024-11-06T04:37:16Z- Weighting particles with subcloud comparison 2024-11-06T04:37:16Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:16Z- meanExpectedMeasDiff: -3.282285 delta_rms_squared: 2.290932 alpha: 0.991345 2024-11-06T04:37:16Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:16Z- meanExpectedMeasDiff: -2.357899 delta_rms_squared: 2.195040 alpha: 0.990971 2024-11-06T04:37:16Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:16Z- meanExpectedMeasDiff: 3.544608 delta_rms_squared: 2.262159 alpha: 0.991236 2024-11-06T04:37:17Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:17Z- meanExpectedMeasDiff: 0.147909 delta_rms_squared: 1.872084 alpha: 0.989430 2024-11-06T04:37:17Z- Subcloud NIS: 0.330931 2024-11-06T04:37:17Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:37:17Z- TNavPF:: sumWeights = 0.178834 2024-11-06T04:37:17Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:37:17Z- TNavPF::First Term of Map Covariance Matrix is 1.25 2024-11-06T04:37:17Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.01 2024-11-06T04:37:17Z- TerrainNav::Resampling particle filter... 2024-11-06T04:37:17Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730867836.72 sec, ping # 0. 2024-11-06T04:37:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:37:17Z- variances([0], [2], and psi_berg) = 219.373277 166.462147 0.000000 2024-11-06T04:37:17Z- publishEstimatesDecoder() - at 1730867836.72 sec 2024-11-06T04:37:17Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:37:17Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061559.77 594434.32 23.23 2024-11-06T04:37:17Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061549.67 594442.51 24.84 lat/lon: 36.69 -121.94 2024-11-06T04:37:17Z- LcmTrn::publishEstimatesDecoder() - COVAR: 219.37 166.46 1.55 0.00 2024-11-06T04:37:17Z- AHRS_M2 msg: 1730867837.11 epoch sec; seqNo:269 2024-11-06T04:37:17Z- AHRS_M2 msg: 0.09 phi; -0.23 theta; 5.49 psi 2024-11-06T04:37:17Z- handleDepth()- Depth_Keller msg: 1730867837.12 epoch sec; seqNo:297; depth 17.61 2024-11-06T04:37:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867837.13 epoch sec; seqNo:297; 4061543.98 north; 594393.13 east; 36.69 lat; -121.94 lon; 17.61 depth; utmZone: 10 2024-11-06T04:37:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867836.72 2024-11-06T04:37:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:17Z- waiting for 1730867840.72, time is 1730867837.13 2024-11-06T04:37:17Z- AHRS_M2 msg: 1730867837.50 epoch sec; seqNo:270 2024-11-06T04:37:17Z- AHRS_M2 msg: 0.09 phi; -0.23 theta; 5.49 psi 2024-11-06T04:37:17Z- handleDepth()- Depth_Keller msg: 1730867837.51 epoch sec; seqNo:298; depth 17.69 2024-11-06T04:37:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867837.52 epoch sec; seqNo:298; 4061544.36 north; 594393.12 east; 36.69 lat; -121.94 lon; 17.69 depth; utmZone: 10 2024-11-06T04:37:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867836.72 2024-11-06T04:37:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:17Z- waiting for 1730867840.72, time is 1730867837.52 2024-11-06T04:37:17Z- AHRS_M2 msg: 1730867837.91 epoch sec; seqNo:271 2024-11-06T04:37:17Z- AHRS_M2 msg: 0.09 phi; -0.23 theta; 5.48 psi 2024-11-06T04:37:17Z- handleDepth()- Depth_Keller msg: 1730867837.91 epoch sec; seqNo:299; depth 17.77 2024-11-06T04:37:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867837.92 epoch sec; seqNo:299; 4061544.36 north; 594393.12 east; 36.69 lat; -121.94 lon; 17.77 depth; utmZone: 10 2024-11-06T04:37:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867836.72 2024-11-06T04:37:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:18Z- waiting for 1730867840.72, time is 1730867837.92 2024-11-06T04:37:18Z- AHRS_M2 msg: 1730867838.32 epoch sec; seqNo:272 2024-11-06T04:37:18Z- AHRS_M2 msg: 0.09 phi; -0.23 theta; 5.48 psi 2024-11-06T04:37:18Z- handleDepth()- Depth_Keller msg: 1730867838.33 epoch sec; seqNo:300; depth 17.87 2024-11-06T04:37:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867838.34 epoch sec; seqNo:300; 4061544.73 north; 594391.90 east; 36.69 lat; -121.94 lon; 17.87 depth; utmZone: 10 2024-11-06T04:37:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867836.72 2024-11-06T04:37:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:18Z- waiting for 1730867840.72, time is 1730867838.34 2024-11-06T04:37:18Z- AHRS_M2 msg: 1730867838.71 epoch sec; seqNo:273 2024-11-06T04:37:18Z- AHRS_M2 msg: 0.09 phi; -0.24 theta; 5.47 psi 2024-11-06T04:37:18Z- handleDepth()- Depth_Keller msg: 1730867838.72 epoch sec; seqNo:301; depth 17.95 2024-11-06T04:37:18Z- handleDVL() - DVL msg epochMillisec: 1730867838.74 2024-11-06T04:37:18Z- handleDvl() - RDI_Pathfinder msg: 1730867838.74 sec; seqNo:101; #beams:4 2024-11-06T04:37:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.09 , 67.46 , 65.01 , 85.00 2024-11-06T04:37:18Z- handleDvl() - RDI_Pathfinder msg: velocities 1.15 , -0.08 , 0.16 2024-11-06T04:37:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867838.76 epoch sec; seqNo:301; 4061545.10 north; 594391.89 east; 36.69 lat; -121.94 lon; 17.95 depth; utmZone: 10 2024-11-06T04:37:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867836.72 2024-11-06T04:37:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:18Z- waiting for 1730867840.72, time is 1730867838.76 2024-11-06T04:37:19Z- handleDepth()- Depth_Keller msg: 1730867839.14 epoch sec; seqNo:302; depth 18.06 2024-11-06T04:37:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867839.16 epoch sec; seqNo:302; 4061545.48 north; 594391.89 east; 36.69 lat; -121.94 lon; 18.06 depth; utmZone: 10 2024-11-06T04:37:19Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:37:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867836.72 2024-11-06T04:37:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:19Z- waiting for 1730867840.72, time is 1730867839.16 2024-11-06T04:37:19Z- AHRS_M2 msg: 1730867839.53 epoch sec; seqNo:274 2024-11-06T04:37:19Z- AHRS_M2 msg: 0.09 phi; -0.24 theta; 5.46 psi 2024-11-06T04:37:19Z- handleDepth()- Depth_Keller msg: 1730867839.53 epoch sec; seqNo:303; depth 18.14 2024-11-06T04:37:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867839.54 epoch sec; seqNo:303; 4061545.48 north; 594391.89 east; 36.69 lat; -121.94 lon; 18.14 depth; utmZone: 10 2024-11-06T04:37:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867836.72 2024-11-06T04:37:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:19Z- waiting for 1730867840.72, time is 1730867839.54 2024-11-06T04:37:19Z- AHRS_M2 msg: 1730867839.93 epoch sec; seqNo:275 2024-11-06T04:37:19Z- AHRS_M2 msg: 0.09 phi; -0.24 theta; 5.45 psi 2024-11-06T04:37:19Z- handleDepth()- Depth_Keller msg: 1730867839.93 epoch sec; seqNo:304; depth 18.22 2024-11-06T04:37:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867839.94 epoch sec; seqNo:304; 4061545.85 north; 594390.67 east; 36.69 lat; -121.94 lon; 18.22 depth; utmZone: 10 2024-11-06T04:37:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867836.72 2024-11-06T04:37:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:20Z- waiting for 1730867840.72, time is 1730867839.94 2024-11-06T04:37:20Z- AHRS_M2 msg: 1730867840.33 epoch sec; seqNo:276 2024-11-06T04:37:20Z- AHRS_M2 msg: 0.09 phi; -0.24 theta; 5.45 psi 2024-11-06T04:37:20Z- handleDepth()- Depth_Keller msg: 1730867840.36 epoch sec; seqNo:305; depth 18.30 2024-11-06T04:37:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867840.41 epoch sec; seqNo:305; 4061546.23 north; 594390.66 east; 36.69 lat; -121.94 lon; 18.30 depth; utmZone: 10 2024-11-06T04:37:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867836.72 2024-11-06T04:37:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:20Z- waiting for 1730867840.72, time is 1730867840.41 2024-11-06T04:37:20Z- AHRS_M2 msg: 1730867840.73 epoch sec; seqNo:277 2024-11-06T04:37:20Z- AHRS_M2 msg: 0.09 phi; -0.24 theta; 5.44 psi 2024-11-06T04:37:20Z- handleDepth()- Depth_Keller msg: 1730867840.74 epoch sec; seqNo:306; depth 18.42 2024-11-06T04:37:20Z- handleDVL() - DVL msg epochMillisec: 1730867840.76 2024-11-06T04:37:20Z- handleDvl() - RDI_Pathfinder msg: 1730867840.76 sec; seqNo:102; #beams:4 2024-11-06T04:37:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.09 , 67.87 , 65.42 , 87.17 2024-11-06T04:37:20Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , 0.07 , 0.20 2024-11-06T04:37:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867840.78 epoch sec; seqNo:306; 4061546.61 north; 594390.66 east; 36.69 lat; -121.94 lon; 18.42 depth; utmZone: 10 2024-11-06T04:37:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867836.72 2024-11-06T04:37:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:20Z- waiting for 1730867840.72, time is 1730867840.78 2024-11-06T04:37:20Z- LcmTrn::updateTrn() >>>> pose time 1730867840.78, meas time 1730867840.76 <<<< 2024-11-06T04:37:20Z- LcmTrn::updateTrn() equating pose 1730867840.76 and meas times 1730867840.76 2024-11-06T04:37:20Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867840.76 2024-11-06T04:37:20Z- TerrainNav::motionUpdate() - currEstimate = (4061547, 594390.66, 18.42) 2024-11-06T04:37:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:37:20Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:37:20Z- TNavPF::Projecting Measurements 2024-11-06T04:37:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:37:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:37:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:37:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:37:20Z- TNavFilter::4 good beams. 2024-11-06T04:37:20Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:37:20Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:37:20Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:37:20Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:37:20Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:37:20Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:37:20Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:37:20Z- TerrainMapOctree: (vehN, vehE ) = (4061546.61, 594390.66). 2024-11-06T04:37:20Z- TerrainMapOctree: Min Distance = 247.29. 2024-11-06T04:37:20Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:37:21Z- Forcing Subcloud Comparison 2024-11-06T04:37:21Z- Weighting particles with subcloud comparison 2024-11-06T04:37:21Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:21Z- meanExpectedMeasDiff: -2.217113 delta_rms_squared: 1.838555 alpha: 0.989239 2024-11-06T04:37:21Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:21Z- meanExpectedMeasDiff: -1.019234 delta_rms_squared: 3.441724 alpha: 0.994223 2024-11-06T04:37:21Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:21Z- meanExpectedMeasDiff: 4.199246 delta_rms_squared: 4.436301 alpha: 0.995512 2024-11-06T04:37:21Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:21Z- meanExpectedMeasDiff: 0.594969 delta_rms_squared: 2.141694 alpha: 0.990748 2024-11-06T04:37:21Z- Subcloud NIS: 0.356015 2024-11-06T04:37:21Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:37:21Z- TNavPF:: sumWeights = 0.213832 2024-11-06T04:37:21Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:37:21Z- TNavPF::First Term of Map Covariance Matrix is 0.80 2024-11-06T04:37:21Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T04:37:21Z- TerrainNav::Resampling particle filter... 2024-11-06T04:37:21Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730867840.76 sec, ping # 0. 2024-11-06T04:37:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:37:21Z- variances([0], [2], and psi_berg) = 278.418321 204.941789 0.000000 2024-11-06T04:37:21Z- publishEstimatesDecoder() - at 1730867840.76 sec 2024-11-06T04:37:21Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:37:21Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061544.68 594441.68 25.33 2024-11-06T04:37:21Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061540.68 594427.81 26.45 lat/lon: 36.69 -121.94 2024-11-06T04:37:21Z- LcmTrn::publishEstimatesDecoder() - COVAR: 278.42 204.94 1.74 0.00 2024-11-06T04:37:21Z- AHRS_M2 msg: 1730867841.17 epoch sec; seqNo:278 2024-11-06T04:37:21Z- AHRS_M2 msg: 0.09 phi; -0.24 theta; 5.44 psi 2024-11-06T04:37:21Z- handleDepth()- Depth_Keller msg: 1730867841.18 epoch sec; seqNo:307; depth 18.50 2024-11-06T04:37:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867841.20 epoch sec; seqNo:307; 4061546.61 north; 594390.66 east; 36.69 lat; -121.94 lon; 18.50 depth; utmZone: 10 2024-11-06T04:37:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867840.76 2024-11-06T04:37:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:21Z- waiting for 1730867844.76, time is 1730867841.20 2024-11-06T04:37:21Z- AHRS_M2 msg: 1730867841.54 epoch sec; seqNo:279 2024-11-06T04:37:21Z- AHRS_M2 msg: 0.09 phi; -0.24 theta; 5.43 psi 2024-11-06T04:37:21Z- handleDepth()- Depth_Keller msg: 1730867841.55 epoch sec; seqNo:308; depth 18.59 2024-11-06T04:37:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867841.56 epoch sec; seqNo:308; 4061546.97 north; 594389.43 east; 36.69 lat; -121.94 lon; 18.59 depth; utmZone: 10 2024-11-06T04:37:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867840.76 2024-11-06T04:37:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:21Z- waiting for 1730867844.76, time is 1730867841.56 2024-11-06T04:37:21Z- AHRS_M2 msg: 1730867841.94 epoch sec; seqNo:280 2024-11-06T04:37:21Z- AHRS_M2 msg: 0.09 phi; -0.24 theta; 5.42 psi 2024-11-06T04:37:21Z- handleDepth()- Depth_Keller msg: 1730867841.96 epoch sec; seqNo:309; depth 18.68 2024-11-06T04:37:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867841.96 epoch sec; seqNo:309; 4061547.35 north; 594389.43 east; 36.69 lat; -121.94 lon; 18.68 depth; utmZone: 10 2024-11-06T04:37:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867840.76 2024-11-06T04:37:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:22Z- waiting for 1730867844.76, time is 1730867841.96 2024-11-06T04:37:22Z- AHRS_M2 msg: 1730867842.38 epoch sec; seqNo:281 2024-11-06T04:37:22Z- AHRS_M2 msg: 0.09 phi; -0.25 theta; 5.41 psi 2024-11-06T04:37:22Z- handleDepth()- Depth_Keller msg: 1730867842.40 epoch sec; seqNo:310; depth 18.77 2024-11-06T04:37:22Z- handleDVL() - DVL msg epochMillisec: 1730867842.43 2024-11-06T04:37:22Z- handleDvl() - RDI_Pathfinder msg: 1730867842.43 sec; seqNo:103; #beams:4 2024-11-06T04:37:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.68 , 67.46 , 65.82 , 87.17 2024-11-06T04:37:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.01 , 0.18 2024-11-06T04:37:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867842.45 epoch sec; seqNo:310; 4061547.35 north; 594389.43 east; 36.69 lat; -121.94 lon; 18.77 depth; utmZone: 10 2024-11-06T04:37:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867840.76 2024-11-06T04:37:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:22Z- waiting for 1730867844.76, time is 1730867842.45 2024-11-06T04:37:22Z- AHRS_M2 msg: 1730867842.75 epoch sec; seqNo:282 2024-11-06T04:37:22Z- AHRS_M2 msg: 0.09 phi; -0.25 theta; 5.41 psi 2024-11-06T04:37:22Z- handleDepth()- Depth_Keller msg: 1730867842.76 epoch sec; seqNo:311; depth 18.89 2024-11-06T04:37:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867842.78 epoch sec; seqNo:311; 4061547.73 north; 594389.42 east; 36.69 lat; -121.94 lon; 18.89 depth; utmZone: 10 2024-11-06T04:37:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867840.76 2024-11-06T04:37:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:22Z- waiting for 1730867844.76, time is 1730867842.78 2024-11-06T04:37:23Z- handleDepth()- Depth_Keller msg: 1730867843.18 epoch sec; seqNo:312; depth 18.96 2024-11-06T04:37:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867843.19 epoch sec; seqNo:312; 4061547.73 north; 594389.42 east; 36.69 lat; -121.94 lon; 18.96 depth; utmZone: 10 2024-11-06T04:37:23Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:37:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867840.76 2024-11-06T04:37:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:23Z- waiting for 1730867844.76, time is 1730867843.19 2024-11-06T04:37:23Z- AHRS_M2 msg: 1730867843.56 epoch sec; seqNo:283 2024-11-06T04:37:23Z- AHRS_M2 msg: 0.08 phi; -0.25 theta; 5.40 psi 2024-11-06T04:37:23Z- handleDepth()- Depth_Keller msg: 1730867843.57 epoch sec; seqNo:313; depth 19.06 2024-11-06T04:37:23Z- handleDVL() - DVL msg epochMillisec: 1730867843.57 2024-11-06T04:37:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:37:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:37:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:37:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:37:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:37:23Z- handleDvl() - RDI_Pathfinder msg: 1730867843.57 sec; seqNo:104; #beams:0 2024-11-06T04:37:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:37:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.01 , 0.18 2024-11-06T04:37:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867843.58 epoch sec; seqNo:313; 4061548.09 north; 594388.20 east; 36.69 lat; -121.94 lon; 19.06 depth; utmZone: 10 2024-11-06T04:37:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867840.76 2024-11-06T04:37:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:23Z- waiting for 1730867844.76, time is 1730867843.58 2024-11-06T04:37:23Z- AHRS_M2 msg: 1730867843.96 epoch sec; seqNo:284 2024-11-06T04:37:23Z- AHRS_M2 msg: 0.08 phi; -0.26 theta; 5.40 psi 2024-11-06T04:37:23Z- handleDepth()- Depth_Keller msg: 1730867843.97 epoch sec; seqNo:314; depth 19.13 2024-11-06T04:37:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867843.98 epoch sec; seqNo:314; 4061548.09 north; 594388.20 east; 36.69 lat; -121.94 lon; 19.13 depth; utmZone: 10 2024-11-06T04:37:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867840.76 2024-11-06T04:37:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:24Z- waiting for 1730867844.76, time is 1730867843.98 2024-11-06T04:37:24Z- AHRS_M2 msg: 1730867844.41 epoch sec; seqNo:285 2024-11-06T04:37:24Z- AHRS_M2 msg: 0.08 phi; -0.26 theta; 5.40 psi 2024-11-06T04:37:24Z- handleDepth()- Depth_Keller msg: 1730867844.42 epoch sec; seqNo:315; depth 19.21 2024-11-06T04:37:24Z- handleDVL() - DVL msg epochMillisec: 1730867844.44 2024-11-06T04:37:24Z- handleDvl() - RDI_Pathfinder msg: 1730867844.44 sec; seqNo:105; #beams:4 2024-11-06T04:37:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.27 , 67.87 , 64.60 , 87.17 2024-11-06T04:37:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , 0.04 , -0.06 2024-11-06T04:37:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867844.47 epoch sec; seqNo:315; 4061548.47 north; 594388.20 east; 36.69 lat; -121.94 lon; 19.21 depth; utmZone: 10 2024-11-06T04:37:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867840.76 2024-11-06T04:37:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:24Z- waiting for 1730867844.76, time is 1730867844.47 2024-11-06T04:37:24Z- AHRS_M2 msg: 1730867844.77 epoch sec; seqNo:286 2024-11-06T04:37:24Z- AHRS_M2 msg: 0.08 phi; -0.26 theta; 5.40 psi 2024-11-06T04:37:24Z- handleDepth()- Depth_Keller msg: 1730867844.78 epoch sec; seqNo:316; depth 19.32 2024-11-06T04:37:24Z- handleDVL() - DVL msg epochMillisec: 1730867844.79 2024-11-06T04:37:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:37:24Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:37:24Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:37:24Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:37:24Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:37:24Z- handleDvl() - RDI_Pathfinder msg: 1730867844.79 sec; seqNo:106; #beams:0 2024-11-06T04:37:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:37:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.66 , 0.04 , -0.06 2024-11-06T04:37:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867844.81 epoch sec; seqNo:316; 4061548.47 north; 594388.20 east; 36.69 lat; -121.94 lon; 19.32 depth; utmZone: 10 2024-11-06T04:37:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867840.76 2024-11-06T04:37:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:24Z- waiting for 1730867844.76, time is 1730867844.81 2024-11-06T04:37:24Z- LcmTrn::updateTrn() >>>> pose time 1730867844.81, meas time 1730867844.79 <<<< 2024-11-06T04:37:24Z- LcmTrn::updateTrn() equating pose 1730867844.79 and meas times 1730867844.79 2024-11-06T04:37:24Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867844.79 2024-11-06T04:37:24Z- TerrainNav::motionUpdate() - currEstimate = (4061548, 594388.20, 19.32) 2024-11-06T04:37:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:37:24Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:37:24Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:37:24Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:37:24Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:37:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:37:24Z- variances([0], [2], and psi_berg) = 278.423268 204.943902 0.000000 2024-11-06T04:37:24Z- publishEstimatesDecoder() - at 1730867844.79 sec 2024-11-06T04:37:24Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:37:24Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061546.51 594439.22 25.81 2024-11-06T04:37:24Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061542.54 594425.35 27.36 lat/lon: 36.69 -121.94 2024-11-06T04:37:24Z- LcmTrn::publishEstimatesDecoder() - COVAR: 278.42 204.94 1.99 0.00 2024-11-06T04:37:25Z- AHRS_M2 msg: 1730867845.18 epoch sec; seqNo:287 2024-11-06T04:37:25Z- AHRS_M2 msg: 0.08 phi; -0.26 theta; 5.40 psi 2024-11-06T04:37:25Z- handleDepth()- Depth_Keller msg: 1730867845.19 epoch sec; seqNo:317; depth 19.38 2024-11-06T04:37:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867845.20 epoch sec; seqNo:317; 4061548.85 north; 594388.19 east; 36.69 lat; -121.94 lon; 19.38 depth; utmZone: 10 2024-11-06T04:37:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867844.79 2024-11-06T04:37:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:25Z- waiting for 1730867848.79, time is 1730867845.20 2024-11-06T04:37:25Z- AHRS_M2 msg: 1730867845.58 epoch sec; seqNo:288 2024-11-06T04:37:25Z- AHRS_M2 msg: 0.08 phi; -0.25 theta; 5.40 psi 2024-11-06T04:37:25Z- handleDepth()- Depth_Keller msg: 1730867845.58 epoch sec; seqNo:318; depth 19.45 2024-11-06T04:37:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867845.60 epoch sec; seqNo:318; 4061548.85 north; 594388.19 east; 36.69 lat; -121.94 lon; 19.45 depth; utmZone: 10 2024-11-06T04:37:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867844.79 2024-11-06T04:37:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:25Z- waiting for 1730867848.79, time is 1730867845.60 2024-11-06T04:37:25Z- AHRS_M2 msg: 1730867845.99 epoch sec; seqNo:289 2024-11-06T04:37:26Z- AHRS_M2 msg: 0.08 phi; -0.25 theta; 5.40 psi 2024-11-06T04:37:26Z- handleDepth()- Depth_Keller msg: 1730867846.00 epoch sec; seqNo:319; depth 19.53 2024-11-06T04:37:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867846.02 epoch sec; seqNo:319; 4061549.22 north; 594386.97 east; 36.69 lat; -121.94 lon; 19.53 depth; utmZone: 10 2024-11-06T04:37:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867844.79 2024-11-06T04:37:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:26Z- waiting for 1730867848.79, time is 1730867846.02 2024-11-06T04:37:26Z- AHRS_M2 msg: 1730867846.42 epoch sec; seqNo:290 2024-11-06T04:37:26Z- AHRS_M2 msg: 0.08 phi; -0.24 theta; 5.40 psi 2024-11-06T04:37:26Z- handleDepth()- Depth_Keller msg: 1730867846.42 epoch sec; seqNo:320; depth 19.61 2024-11-06T04:37:26Z- handleDVL() - DVL msg epochMillisec: 1730867846.44 2024-11-06T04:37:26Z- handleDvl() - RDI_Pathfinder msg: 1730867846.44 sec; seqNo:107; #beams:4 2024-11-06T04:37:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.45 , 68.28 , 64.19 , 87.17 2024-11-06T04:37:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.05 , -0.34 2024-11-06T04:37:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867846.47 epoch sec; seqNo:320; 4061549.22 north; 594386.97 east; 36.69 lat; -121.94 lon; 19.61 depth; utmZone: 10 2024-11-06T04:37:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867844.79 2024-11-06T04:37:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:26Z- waiting for 1730867848.79, time is 1730867846.47 2024-11-06T04:37:26Z- AHRS_M2 msg: 1730867846.80 epoch sec; seqNo:291 2024-11-06T04:37:26Z- AHRS_M2 msg: 0.09 phi; -0.24 theta; 5.40 psi 2024-11-06T04:37:26Z- handleDepth()- Depth_Keller msg: 1730867846.80 epoch sec; seqNo:321; depth 19.71 2024-11-06T04:37:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867846.84 epoch sec; seqNo:321; 4061549.60 north; 594386.96 east; 36.69 lat; -121.94 lon; 19.71 depth; utmZone: 10 2024-11-06T04:37:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867844.79 2024-11-06T04:37:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:26Z- waiting for 1730867848.79, time is 1730867846.84 2024-11-06T04:37:27Z- AHRS_M2 msg: 1730867847.20 epoch sec; seqNo:292 2024-11-06T04:37:27Z- AHRS_M2 msg: 0.09 phi; -0.23 theta; 5.40 psi 2024-11-06T04:37:27Z- handleDepth()- Depth_Keller msg: 1730867847.21 epoch sec; seqNo:322; depth 19.78 2024-11-06T04:37:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867847.22 epoch sec; seqNo:322; 4061549.98 north; 594386.96 east; 36.69 lat; -121.94 lon; 19.78 depth; utmZone: 10 2024-11-06T04:37:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867844.79 2024-11-06T04:37:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:27Z- waiting for 1730867848.79, time is 1730867847.22 2024-11-06T04:37:27Z- AHRS_M2 msg: 1730867847.61 epoch sec; seqNo:293 2024-11-06T04:37:27Z- AHRS_M2 msg: 0.09 phi; -0.23 theta; 5.40 psi 2024-11-06T04:37:27Z- handleDepth()- Depth_Keller msg: 1730867847.61 epoch sec; seqNo:323; depth 19.86 2024-11-06T04:37:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867847.62 epoch sec; seqNo:323; 4061549.96 north; 594385.74 east; 36.69 lat; -121.94 lon; 19.86 depth; utmZone: 10 2024-11-06T04:37:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867844.79 2024-11-06T04:37:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:27Z- waiting for 1730867848.79, time is 1730867847.62 2024-11-06T04:37:28Z- handleDepth()- Depth_Keller msg: 1730867848.02 epoch sec; seqNo:324; depth 19.94 2024-11-06T04:37:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867848.03 epoch sec; seqNo:324; 4061550.34 north; 594385.73 east; 36.69 lat; -121.94 lon; 19.94 depth; utmZone: 10 2024-11-06T04:37:28Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:37:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867844.79 2024-11-06T04:37:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:28Z- waiting for 1730867848.79, time is 1730867848.03 2024-11-06T04:37:28Z- AHRS_M2 msg: 1730867848.42 epoch sec; seqNo:294 2024-11-06T04:37:28Z- AHRS_M2 msg: 0.09 phi; -0.22 theta; 5.39 psi 2024-11-06T04:37:28Z- handleDepth()- Depth_Keller msg: 1730867848.42 epoch sec; seqNo:325; depth 20.03 2024-11-06T04:37:28Z- handleDVL() - DVL msg epochMillisec: 1730867848.45 2024-11-06T04:37:28Z- handleDvl() - RDI_Pathfinder msg: 1730867848.45 sec; seqNo:108; #beams:4 2024-11-06T04:37:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.22 , 66.64 , 64.18 , 85.00 2024-11-06T04:37:28Z- handleDvl() - RDI_Pathfinder msg: velocities 1.22 , -0.13 , -0.11 2024-11-06T04:37:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867848.46 epoch sec; seqNo:325; 4061550.72 north; 594385.73 east; 36.69 lat; -121.94 lon; 20.03 depth; utmZone: 10 2024-11-06T04:37:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867844.79 2024-11-06T04:37:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:28Z- waiting for 1730867848.79, time is 1730867848.46 2024-11-06T04:37:28Z- AHRS_M2 msg: 1730867848.82 epoch sec; seqNo:295 2024-11-06T04:37:28Z- AHRS_M2 msg: 0.09 phi; -0.22 theta; 5.38 psi 2024-11-06T04:37:28Z- handleDepth()- Depth_Keller msg: 1730867848.83 epoch sec; seqNo:326; depth 20.12 2024-11-06T04:37:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867848.87 epoch sec; seqNo:326; 4061550.71 north; 594384.51 east; 36.69 lat; -121.94 lon; 20.12 depth; utmZone: 10 2024-11-06T04:37:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867844.79 2024-11-06T04:37:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:28Z- waiting for 1730867848.79, time is 1730867848.87 2024-11-06T04:37:28Z- LcmTrn::updateTrn() >>>> pose time 1730867848.87, meas time 1730867848.45 <<<< 2024-11-06T04:37:28Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867848.87 2024-11-06T04:37:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867848.45 sec, ping # 0; waiting for INS data more recent than 1730867844.79... 2024-11-06T04:37:28Z- TerrainNav::motionUpdate() - currEstimate = (4061551, 594384.51, 20.12) 2024-11-06T04:37:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:37:28Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:37:29Z- TNavPF::Projecting Measurements 2024-11-06T04:37:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:37:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:37:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:37:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:37:29Z- TNavFilter::4 good beams. 2024-11-06T04:37:29Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:37:29Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:37:29Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:37:29Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:37:29Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:37:29Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:37:29Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:37:29Z- TerrainMapOctree: (vehN, vehE ) = (4061550.48, 594384.89). 2024-11-06T04:37:29Z- TerrainMapOctree: Min Distance = 240.84. 2024-11-06T04:37:29Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:37:29Z- Forcing Subcloud Comparison 2024-11-06T04:37:29Z- Weighting particles with subcloud comparison 2024-11-06T04:37:29Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:29Z- meanExpectedMeasDiff: -2.727313 delta_rms_squared: 2.073269 alpha: 0.990446 2024-11-06T04:37:29Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:29Z- meanExpectedMeasDiff: 2.694917 delta_rms_squared: 8.071488 alpha: 0.997528 2024-11-06T04:37:29Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:29Z- meanExpectedMeasDiff: 2.088311 delta_rms_squared: 7.550640 alpha: 0.997358 2024-11-06T04:37:29Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:29Z- meanExpectedMeasDiff: 3.091808 delta_rms_squared: 2.298842 alpha: 0.991375 2024-11-06T04:37:29Z- Subcloud NIS: 0.388371 2024-11-06T04:37:29Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:37:29Z- TNavPF:: sumWeights = 0.115337 2024-11-06T04:37:29Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:37:29Z- TNavPF::First Term of Map Covariance Matrix is 1.02 2024-11-06T04:37:29Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T04:37:29Z- TerrainNav::Resampling particle filter... 2024-11-06T04:37:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867848.45 sec. 2024-11-06T04:37:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:37:29Z- variances([0], [2], and psi_berg) = 315.875062 186.945356 0.000000 2024-11-06T04:37:29Z- publishEstimatesDecoder() - at 1730867848.87 sec 2024-11-06T04:37:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:37:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061548.66 594435.45 25.76 2024-11-06T04:37:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061538.75 594416.70 28.13 lat/lon: 36.69 -121.94 2024-11-06T04:37:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 315.88 186.95 2.02 0.00 2024-11-06T04:37:29Z- AHRS_M2 msg: 1730867849.22 epoch sec; seqNo:296 2024-11-06T04:37:29Z- AHRS_M2 msg: 0.10 phi; -0.21 theta; 5.37 psi 2024-11-06T04:37:29Z- handleDepth()- Depth_Keller msg: 1730867849.22 epoch sec; seqNo:327; depth 20.19 2024-11-06T04:37:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867849.23 epoch sec; seqNo:327; 4061551.09 north; 594384.51 east; 36.69 lat; -121.94 lon; 20.19 depth; utmZone: 10 2024-11-06T04:37:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867848.87 2024-11-06T04:37:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:29Z- waiting for 1730867852.87, time is 1730867849.23 2024-11-06T04:37:29Z- AHRS_M2 msg: 1730867849.63 epoch sec; seqNo:297 2024-11-06T04:37:29Z- AHRS_M2 msg: 0.10 phi; -0.22 theta; 5.36 psi 2024-11-06T04:37:29Z- handleDepth()- Depth_Keller msg: 1730867849.64 epoch sec; seqNo:328; depth 20.28 2024-11-06T04:37:29Z- handleDVL() - DVL msg epochMillisec: 1730867849.64 2024-11-06T04:37:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:37:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:37:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:37:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:37:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:37:29Z- handleDvl() - RDI_Pathfinder msg: 1730867849.64 sec; seqNo:109; #beams:0 2024-11-06T04:37:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:37:29Z- handleDvl() - RDI_Pathfinder msg: velocities 1.22 , -0.13 , -0.11 2024-11-06T04:37:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867849.66 epoch sec; seqNo:328; 4061551.46 north; 594384.50 east; 36.69 lat; -121.94 lon; 20.28 depth; utmZone: 10 2024-11-06T04:37:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867848.87 2024-11-06T04:37:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:29Z- waiting for 1730867852.87, time is 1730867849.66 2024-11-06T04:37:30Z- AHRS_M2 msg: 1730867850.02 epoch sec; seqNo:298 2024-11-06T04:37:30Z- AHRS_M2 msg: 0.10 phi; -0.22 theta; 5.35 psi 2024-11-06T04:37:30Z- handleDepth()- Depth_Keller msg: 1730867850.03 epoch sec; seqNo:329; depth 20.38 2024-11-06T04:37:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867850.04 epoch sec; seqNo:329; 4061551.45 north; 594383.28 east; 36.69 lat; -121.94 lon; 20.38 depth; utmZone: 10 2024-11-06T04:37:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867848.87 2024-11-06T04:37:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:30Z- waiting for 1730867852.87, time is 1730867850.04 2024-11-06T04:37:30Z- AHRS_M2 msg: 1730867850.45 epoch sec; seqNo:299 2024-11-06T04:37:30Z- AHRS_M2 msg: 0.10 phi; -0.22 theta; 5.34 psi 2024-11-06T04:37:30Z- handleDepth()- Depth_Keller msg: 1730867850.46 epoch sec; seqNo:330; depth 20.45 2024-11-06T04:37:30Z- handleDVL() - DVL msg epochMillisec: 1730867850.49 2024-11-06T04:37:30Z- handleDvl() - RDI_Pathfinder msg: 1730867850.49 sec; seqNo:110; #beams:4 2024-11-06T04:37:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.63 , 65.41 , 64.18 , 79.31 2024-11-06T04:37:30Z- handleDvl() - RDI_Pathfinder msg: velocities 1.10 , -0.03 , 0.19 2024-11-06T04:37:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867850.50 epoch sec; seqNo:330; 4061551.83 north; 594383.28 east; 36.69 lat; -121.94 lon; 20.45 depth; utmZone: 10 2024-11-06T04:37:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867848.87 2024-11-06T04:37:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:30Z- waiting for 1730867852.87, time is 1730867850.50 2024-11-06T04:37:30Z- AHRS_M2 msg: 1730867850.84 epoch sec; seqNo:300 2024-11-06T04:37:30Z- AHRS_M2 msg: 0.10 phi; -0.22 theta; 5.33 psi 2024-11-06T04:37:30Z- handleDepth()- Depth_Keller msg: 1730867850.86 epoch sec; seqNo:331; depth 20.55 2024-11-06T04:37:30Z- handleDVL() - DVL msg epochMillisec: 1730867850.87 2024-11-06T04:37:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:37:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:37:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:37:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:37:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:37:30Z- handleDvl() - RDI_Pathfinder msg: 1730867850.87 sec; seqNo:111; #beams:0 2024-11-06T04:37:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:37:30Z- handleDvl() - RDI_Pathfinder msg: velocities 1.10 , -0.03 , 0.19 2024-11-06T04:37:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867850.88 epoch sec; seqNo:331; 4061552.21 north; 594383.27 east; 36.69 lat; -121.94 lon; 20.55 depth; utmZone: 10 2024-11-06T04:37:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867848.87 2024-11-06T04:37:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:31Z- waiting for 1730867852.87, time is 1730867850.88 2024-11-06T04:37:31Z- handleDepth()- Depth_Keller msg: 1730867851.25 epoch sec; seqNo:332; depth 20.64 2024-11-06T04:37:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867851.27 epoch sec; seqNo:332; 4061552.21 north; 594383.27 east; 36.69 lat; -121.94 lon; 20.64 depth; utmZone: 10 2024-11-06T04:37:31Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:37:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867848.87 2024-11-06T04:37:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:31Z- waiting for 1730867852.87, time is 1730867851.27 2024-11-06T04:37:31Z- AHRS_M2 msg: 1730867851.65 epoch sec; seqNo:301 2024-11-06T04:37:31Z- AHRS_M2 msg: 0.10 phi; -0.23 theta; 5.32 psi 2024-11-06T04:37:31Z- handleDepth()- Depth_Keller msg: 1730867851.66 epoch sec; seqNo:333; depth 20.70 2024-11-06T04:37:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867851.68 epoch sec; seqNo:333; 4061552.57 north; 594382.05 east; 36.69 lat; -121.94 lon; 20.70 depth; utmZone: 10 2024-11-06T04:37:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867848.87 2024-11-06T04:37:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:31Z- waiting for 1730867852.87, time is 1730867851.68 2024-11-06T04:37:32Z- AHRS_M2 msg: 1730867852.05 epoch sec; seqNo:302 2024-11-06T04:37:32Z- AHRS_M2 msg: 0.11 phi; -0.23 theta; 5.32 psi 2024-11-06T04:37:32Z- handleDepth()- Depth_Keller msg: 1730867852.05 epoch sec; seqNo:334; depth 20.79 2024-11-06T04:37:32Z- handleDVL() - DVL msg epochMillisec: 1730867852.05 2024-11-06T04:37:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:37:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:37:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:37:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:37:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:37:32Z- handleDvl() - RDI_Pathfinder msg: 1730867852.05 sec; seqNo:112; #beams:0 2024-11-06T04:37:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:37:32Z- handleDvl() - RDI_Pathfinder msg: velocities 1.10 , -0.03 , 0.19 2024-11-06T04:37:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867852.06 epoch sec; seqNo:334; 4061552.57 north; 594382.05 east; 36.69 lat; -121.94 lon; 20.79 depth; utmZone: 10 2024-11-06T04:37:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867848.87 2024-11-06T04:37:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:32Z- waiting for 1730867852.87, time is 1730867852.06 2024-11-06T04:37:32Z- AHRS_M2 msg: 1730867852.45 epoch sec; seqNo:303 2024-11-06T04:37:32Z- AHRS_M2 msg: 0.11 phi; -0.23 theta; 5.32 psi 2024-11-06T04:37:32Z- handleDepth()- Depth_Keller msg: 1730867852.45 epoch sec; seqNo:335; depth 20.89 2024-11-06T04:37:32Z- handleDVL() - DVL msg epochMillisec: 1730867852.48 2024-11-06T04:37:32Z- handleDvl() - RDI_Pathfinder msg: 1730867852.48 sec; seqNo:113; #beams:4 2024-11-06T04:37:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.00 , 65.44 , 64.07 , 79.41 2024-11-06T04:37:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.02 , 0.17 2024-11-06T04:37:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867852.50 epoch sec; seqNo:335; 4061552.95 north; 594382.04 east; 36.69 lat; -121.94 lon; 20.89 depth; utmZone: 10 2024-11-06T04:37:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867848.87 2024-11-06T04:37:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:32Z- waiting for 1730867852.87, time is 1730867852.50 2024-11-06T04:37:32Z- AHRS_M2 msg: 1730867852.85 epoch sec; seqNo:304 2024-11-06T04:37:32Z- AHRS_M2 msg: 0.12 phi; -0.23 theta; 5.32 psi 2024-11-06T04:37:32Z- handleDepth()- Depth_Keller msg: 1730867852.86 epoch sec; seqNo:336; depth 20.96 2024-11-06T04:37:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867852.90 epoch sec; seqNo:336; 4061552.94 north; 594380.82 east; 36.69 lat; -121.94 lon; 20.96 depth; utmZone: 10 2024-11-06T04:37:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867848.87 2024-11-06T04:37:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:33Z- waiting for 1730867852.87, time is 1730867852.90 2024-11-06T04:37:33Z- LcmTrn::updateTrn() >>>> pose time 1730867852.90, meas time 1730867852.48 <<<< 2024-11-06T04:37:33Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867852.90 2024-11-06T04:37:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867852.48 sec, ping # 0; waiting for INS data more recent than 1730867848.87... 2024-11-06T04:37:33Z- TerrainNav::motionUpdate() - currEstimate = (4061553, 594380.82, 20.96) 2024-11-06T04:37:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:37:33Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:37:33Z- TNavPF::Projecting Measurements 2024-11-06T04:37:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:37:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:37:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:37:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:37:33Z- TNavFilter::4 good beams. 2024-11-06T04:37:33Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:37:33Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:37:33Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:37:33Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:37:33Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:37:33Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:37:33Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:37:33Z- TerrainMapOctree: (vehN, vehE ) = (4061552.71, 594381.20). 2024-11-06T04:37:33Z- TerrainMapOctree: Min Distance = 236.93. 2024-11-06T04:37:33Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:37:33Z- Forcing Subcloud Comparison 2024-11-06T04:37:33Z- Weighting particles with subcloud comparison 2024-11-06T04:37:33Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:33Z- meanExpectedMeasDiff: -2.720249 delta_rms_squared: 2.453279 alpha: 0.991914 2024-11-06T04:37:33Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:33Z- meanExpectedMeasDiff: 3.048337 delta_rms_squared: 6.315550 alpha: 0.996843 2024-11-06T04:37:33Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:33Z- meanExpectedMeasDiff: 1.048974 delta_rms_squared: 7.333716 alpha: 0.997280 2024-11-06T04:37:33Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:33Z- meanExpectedMeasDiff: -2.888858 delta_rms_squared: 2.585751 alpha: 0.992325 2024-11-06T04:37:33Z- Subcloud NIS: 0.401870 2024-11-06T04:37:33Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:37:33Z- TNavPF:: sumWeights = 0.150429 2024-11-06T04:37:33Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:37:33Z- TNavPF::First Term of Map Covariance Matrix is 0.78 2024-11-06T04:37:33Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T04:37:33Z- TerrainNav::Resampling particle filter... 2024-11-06T04:37:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867852.48 sec. 2024-11-06T04:37:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:37:33Z- variances([0], [2], and psi_berg) = 494.497687 216.462434 0.000000 2024-11-06T04:37:33Z- publishEstimatesDecoder() - at 1730867852.90 sec 2024-11-06T04:37:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:37:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061550.75 594431.78 26.70 2024-11-06T04:37:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061526.65 594403.44 29.88 lat/lon: 36.69 -121.94 2024-11-06T04:37:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 494.50 216.46 2.53 0.00 2024-11-06T04:37:33Z- AHRS_M2 msg: 1730867853.25 epoch sec; seqNo:305 2024-11-06T04:37:33Z- AHRS_M2 msg: 0.12 phi; -0.24 theta; 5.32 psi 2024-11-06T04:37:33Z- handleDepth()- Depth_Keller msg: 1730867853.28 epoch sec; seqNo:337; depth 21.05 2024-11-06T04:37:33Z- handleDVL() - DVL msg epochMillisec: 1730867853.29 2024-11-06T04:37:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:37:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:37:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:37:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:37:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:37:33Z- handleDvl() - RDI_Pathfinder msg: 1730867853.29 sec; seqNo:114; #beams:0 2024-11-06T04:37:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:37:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.02 , 0.17 2024-11-06T04:37:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867853.30 epoch sec; seqNo:337; 4061553.32 north; 594380.82 east; 36.69 lat; -121.94 lon; 21.05 depth; utmZone: 10 2024-11-06T04:37:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867852.90 2024-11-06T04:37:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:33Z- waiting for 1730867856.90, time is 1730867853.30 2024-11-06T04:37:33Z- AHRS_M2 msg: 1730867853.66 epoch sec; seqNo:306 2024-11-06T04:37:33Z- AHRS_M2 msg: 0.13 phi; -0.24 theta; 5.33 psi 2024-11-06T04:37:33Z- handleDepth()- Depth_Keller msg: 1730867853.67 epoch sec; seqNo:338; depth 21.13 2024-11-06T04:37:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867853.69 epoch sec; seqNo:338; 4061553.32 north; 594380.82 east; 36.69 lat; -121.94 lon; 21.13 depth; utmZone: 10 2024-11-06T04:37:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867852.90 2024-11-06T04:37:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:33Z- waiting for 1730867856.90, time is 1730867853.69 2024-11-06T04:37:34Z- AHRS_M2 msg: 1730867854.06 epoch sec; seqNo:307 2024-11-06T04:37:34Z- AHRS_M2 msg: 0.13 phi; -0.24 theta; 5.33 psi 2024-11-06T04:37:34Z- handleDepth()- Depth_Keller msg: 1730867854.07 epoch sec; seqNo:339; depth 21.21 2024-11-06T04:37:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867854.09 epoch sec; seqNo:339; 4061553.70 north; 594380.82 east; 36.69 lat; -121.94 lon; 21.21 depth; utmZone: 10 2024-11-06T04:37:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867852.90 2024-11-06T04:37:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:34Z- waiting for 1730867856.90, time is 1730867854.09 2024-11-06T04:37:34Z- AHRS_M2 msg: 1730867854.47 epoch sec; seqNo:308 2024-11-06T04:37:34Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.34 psi 2024-11-06T04:37:34Z- handleDepth()- Depth_Keller msg: 1730867854.48 epoch sec; seqNo:340; depth 21.30 2024-11-06T04:37:34Z- handleDVL() - DVL msg epochMillisec: 1730867854.51 2024-11-06T04:37:34Z- handleDvl() - RDI_Pathfinder msg: 1730867854.51 sec; seqNo:115; #beams:4 2024-11-06T04:37:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.36 , 65.10 , 63.73 , 80.43 2024-11-06T04:37:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.02 , 0.04 2024-11-06T04:37:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867854.52 epoch sec; seqNo:340; 4061553.70 north; 594380.82 east; 36.69 lat; -121.94 lon; 21.30 depth; utmZone: 10 2024-11-06T04:37:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867852.90 2024-11-06T04:37:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:34Z- waiting for 1730867856.90, time is 1730867854.52 2024-11-06T04:37:34Z- AHRS_M2 msg: 1730867854.88 epoch sec; seqNo:309 2024-11-06T04:37:34Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.34 psi 2024-11-06T04:37:34Z- handleDepth()- Depth_Keller msg: 1730867854.89 epoch sec; seqNo:341; depth 21.39 2024-11-06T04:37:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867854.90 epoch sec; seqNo:341; 4061554.06 north; 594379.59 east; 36.69 lat; -121.94 lon; 21.39 depth; utmZone: 10 2024-11-06T04:37:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867852.90 2024-11-06T04:37:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:35Z- waiting for 1730867856.90, time is 1730867854.90 2024-11-06T04:37:35Z- AHRS_M2 msg: 1730867855.28 epoch sec; seqNo:310 2024-11-06T04:37:35Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.34 psi 2024-11-06T04:37:35Z- handleDepth()- Depth_Keller msg: 1730867855.28 epoch sec; seqNo:342; depth 21.47 2024-11-06T04:37:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867855.30 epoch sec; seqNo:342; 4061554.06 north; 594379.59 east; 36.69 lat; -121.94 lon; 21.47 depth; utmZone: 10 2024-11-06T04:37:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867852.90 2024-11-06T04:37:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:35Z- waiting for 1730867856.90, time is 1730867855.30 2024-11-06T04:37:35Z- AHRS_M2 msg: 1730867855.68 epoch sec; seqNo:311 2024-11-06T04:37:35Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.34 psi 2024-11-06T04:37:35Z- handleDepth()- Depth_Keller msg: 1730867855.69 epoch sec; seqNo:343; depth 21.55 2024-11-06T04:37:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867855.73 epoch sec; seqNo:343; 4061554.44 north; 594379.59 east; 36.69 lat; -121.94 lon; 21.55 depth; utmZone: 10 2024-11-06T04:37:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867852.90 2024-11-06T04:37:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:35Z- waiting for 1730867856.90, time is 1730867855.73 2024-11-06T04:37:36Z- AHRS_M2 msg: 1730867856.09 epoch sec; seqNo:312 2024-11-06T04:37:36Z- AHRS_M2 msg: 0.15 phi; -0.25 theta; 5.34 psi 2024-11-06T04:37:36Z- handleDepth()- Depth_Keller msg: 1730867856.10 epoch sec; seqNo:344; depth 21.65 2024-11-06T04:37:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867856.11 epoch sec; seqNo:344; 4061554.44 north; 594379.59 east; 36.69 lat; -121.94 lon; 21.65 depth; utmZone: 10 2024-11-06T04:37:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867852.90 2024-11-06T04:37:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:36Z- waiting for 1730867856.90, time is 1730867856.11 2024-11-06T04:37:36Z- AHRS_M2 msg: 1730867856.49 epoch sec; seqNo:313 2024-11-06T04:37:36Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.34 psi 2024-11-06T04:37:36Z- handleDepth()- Depth_Keller msg: 1730867856.49 epoch sec; seqNo:345; depth 21.72 2024-11-06T04:37:36Z- handleDVL() - DVL msg epochMillisec: 1730867856.52 2024-11-06T04:37:36Z- handleDvl() - RDI_Pathfinder msg: 1730867856.52 sec; seqNo:116; #beams:4 2024-11-06T04:37:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 78.07 , 64.58 , 62.95 , 85.01 2024-11-06T04:37:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.02 , -0.03 2024-11-06T04:37:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867856.53 epoch sec; seqNo:345; 4061554.81 north; 594378.36 east; 36.69 lat; -121.94 lon; 21.72 depth; utmZone: 10 2024-11-06T04:37:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867852.90 2024-11-06T04:37:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:36Z- waiting for 1730867856.90, time is 1730867856.53 2024-11-06T04:37:36Z- AHRS_M2 msg: 1730867856.89 epoch sec; seqNo:314 2024-11-06T04:37:36Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.34 psi 2024-11-06T04:37:36Z- handleDepth()- Depth_Keller msg: 1730867856.91 epoch sec; seqNo:346; depth 21.82 2024-11-06T04:37:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867856.93 epoch sec; seqNo:346; 4061554.81 north; 594378.36 east; 36.69 lat; -121.94 lon; 21.82 depth; utmZone: 10 2024-11-06T04:37:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867852.90 2024-11-06T04:37:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:37Z- waiting for 1730867856.90, time is 1730867856.93 2024-11-06T04:37:37Z- LcmTrn::updateTrn() >>>> pose time 1730867856.93, meas time 1730867856.52 <<<< 2024-11-06T04:37:37Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867856.93 2024-11-06T04:37:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867856.52 sec, ping # 0; waiting for INS data more recent than 1730867852.90... 2024-11-06T04:37:37Z- TerrainNav::motionUpdate() - currEstimate = (4061555, 594378.36, 21.82) 2024-11-06T04:37:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:37:37Z- TerrainNav:: SubcloudNIS is 0.40, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:37:37Z- TNavPF::Projecting Measurements 2024-11-06T04:37:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:37:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:37:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:37:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:37:37Z- TNavFilter::4 good beams. 2024-11-06T04:37:37Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:37:37Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:37:37Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:37:37Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:37:37Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:37:37Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:37:37Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:37:37Z- TerrainMapOctree: (vehN, vehE ) = (4061554.61, 594378.61). 2024-11-06T04:37:37Z- TerrainMapOctree: Min Distance = 233.92. 2024-11-06T04:37:37Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:37:37Z- Forcing Subcloud Comparison 2024-11-06T04:37:37Z- Weighting particles with subcloud comparison 2024-11-06T04:37:37Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:37Z- meanExpectedMeasDiff: -0.807111 delta_rms_squared: 2.864445 alpha: 0.993066 2024-11-06T04:37:37Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:37Z- meanExpectedMeasDiff: 3.413456 delta_rms_squared: 9.720023 alpha: 0.997947 2024-11-06T04:37:37Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:37Z- meanExpectedMeasDiff: 0.389138 delta_rms_squared: 3.041694 alpha: 0.993468 2024-11-06T04:37:37Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:37Z- meanExpectedMeasDiff: 2.980981 delta_rms_squared: 2.864502 alpha: 0.993066 2024-11-06T04:37:37Z- Subcloud NIS: 0.383603 2024-11-06T04:37:37Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:37:37Z- TNavPF:: sumWeights = 0.217412 2024-11-06T04:37:37Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:37:37Z- TNavPF::First Term of Map Covariance Matrix is 0.61 2024-11-06T04:37:37Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T04:37:37Z- TerrainNav::Resampling particle filter... 2024-11-06T04:37:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867856.52 sec. 2024-11-06T04:37:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:37:37Z- variances([0], [2], and psi_berg) = 532.082232 234.912463 0.000000 2024-11-06T04:37:37Z- publishEstimatesDecoder() - at 1730867856.93 sec 2024-11-06T04:37:37Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:37:37Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061552.64 594429.30 26.74 2024-11-06T04:37:37Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061515.41 594393.20 31.06 lat/lon: 36.69 -121.94 2024-11-06T04:37:37Z- LcmTrn::publishEstimatesDecoder() - COVAR: 532.08 234.91 2.82 0.00 2024-11-06T04:37:37Z- AHRS_M2 msg: 1730867857.30 epoch sec; seqNo:315 2024-11-06T04:37:37Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.34 psi 2024-11-06T04:37:37Z- handleDepth()- Depth_Keller msg: 1730867857.31 epoch sec; seqNo:347; depth 21.92 2024-11-06T04:37:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867857.33 epoch sec; seqNo:347; 4061555.19 north; 594378.36 east; 36.69 lat; -121.94 lon; 21.92 depth; utmZone: 10 2024-11-06T04:37:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867856.93 2024-11-06T04:37:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:37Z- waiting for 1730867860.93, time is 1730867857.33 2024-11-06T04:37:37Z- AHRS_M2 msg: 1730867857.70 epoch sec; seqNo:316 2024-11-06T04:37:37Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.34 psi 2024-11-06T04:37:37Z- handleDepth()- Depth_Keller msg: 1730867857.71 epoch sec; seqNo:348; depth 21.99 2024-11-06T04:37:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867857.72 epoch sec; seqNo:348; 4061555.19 north; 594378.36 east; 36.69 lat; -121.94 lon; 21.99 depth; utmZone: 10 2024-11-06T04:37:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867856.93 2024-11-06T04:37:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:37Z- waiting for 1730867860.93, time is 1730867857.72 2024-11-06T04:37:38Z- AHRS_M2 msg: 1730867858.10 epoch sec; seqNo:317 2024-11-06T04:37:38Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.34 psi 2024-11-06T04:37:38Z- handleDepth()- Depth_Keller msg: 1730867858.11 epoch sec; seqNo:349; depth 22.08 2024-11-06T04:37:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867858.12 epoch sec; seqNo:349; 4061555.56 north; 594378.35 east; 36.69 lat; -121.94 lon; 22.08 depth; utmZone: 10 2024-11-06T04:37:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867856.93 2024-11-06T04:37:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:38Z- waiting for 1730867860.93, time is 1730867858.12 2024-11-06T04:37:38Z- AHRS_M2 msg: 1730867858.51 epoch sec; seqNo:318 2024-11-06T04:37:38Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.34 psi 2024-11-06T04:37:38Z- handleDepth()- Depth_Keller msg: 1730867858.52 epoch sec; seqNo:350; depth 22.15 2024-11-06T04:37:38Z- handleDVL() - DVL msg epochMillisec: 1730867858.54 2024-11-06T04:37:38Z- handleDvl() - RDI_Pathfinder msg: 1730867858.54 sec; seqNo:117; #beams:4 2024-11-06T04:37:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.85 , 64.18 , 62.54 , 0.00 2024-11-06T04:37:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , -0.05 2024-11-06T04:37:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867858.55 epoch sec; seqNo:350; 4061555.55 north; 594377.13 east; 36.69 lat; -121.94 lon; 22.15 depth; utmZone: 10 2024-11-06T04:37:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867856.93 2024-11-06T04:37:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:38Z- waiting for 1730867860.93, time is 1730867858.55 2024-11-06T04:37:38Z- AHRS_M2 msg: 1730867858.91 epoch sec; seqNo:319 2024-11-06T04:37:38Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.34 psi 2024-11-06T04:37:38Z- handleDepth()- Depth_Keller msg: 1730867858.92 epoch sec; seqNo:351; depth 22.25 2024-11-06T04:37:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867858.94 epoch sec; seqNo:351; 4061555.93 north; 594377.13 east; 36.69 lat; -121.94 lon; 22.25 depth; utmZone: 10 2024-11-06T04:37:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867856.93 2024-11-06T04:37:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:39Z- waiting for 1730867860.93, time is 1730867858.94 2024-11-06T04:37:39Z- AHRS_M2 msg: 1730867859.32 epoch sec; seqNo:320 2024-11-06T04:37:39Z- AHRS_M2 msg: 0.15 phi; -0.26 theta; 5.35 psi 2024-11-06T04:37:39Z- handleDepth()- Depth_Keller msg: 1730867859.32 epoch sec; seqNo:352; depth 22.34 2024-11-06T04:37:39Z- handleDVL() - DVL msg epochMillisec: 1730867859.32 2024-11-06T04:37:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:37:39Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:37:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:37:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:37:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:37:39Z- handleDvl() - RDI_Pathfinder msg: 1730867859.32 sec; seqNo:118; #beams:0 2024-11-06T04:37:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:37:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , -0.05 2024-11-06T04:37:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867859.33 epoch sec; seqNo:352; 4061555.93 north; 594377.13 east; 36.69 lat; -121.94 lon; 22.34 depth; utmZone: 10 2024-11-06T04:37:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867856.93 2024-11-06T04:37:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:39Z- waiting for 1730867860.93, time is 1730867859.33 2024-11-06T04:37:39Z- AHRS_M2 msg: 1730867859.73 epoch sec; seqNo:321 2024-11-06T04:37:39Z- AHRS_M2 msg: 0.15 phi; -0.26 theta; 5.35 psi 2024-11-06T04:37:39Z- handleDepth()- Depth_Keller msg: 1730867859.74 epoch sec; seqNo:353; depth 22.41 2024-11-06T04:37:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867859.75 epoch sec; seqNo:353; 4061556.31 north; 594377.13 east; 36.69 lat; -121.94 lon; 22.41 depth; utmZone: 10 2024-11-06T04:37:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867856.93 2024-11-06T04:37:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:39Z- waiting for 1730867860.93, time is 1730867859.75 2024-11-06T04:37:40Z- AHRS_M2 msg: 1730867860.12 epoch sec; seqNo:322 2024-11-06T04:37:40Z- AHRS_M2 msg: 0.15 phi; -0.26 theta; 5.35 psi 2024-11-06T04:37:40Z- handleDepth()- Depth_Keller msg: 1730867860.13 epoch sec; seqNo:354; depth 22.50 2024-11-06T04:37:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867860.17 epoch sec; seqNo:354; 4061556.29 north; 594375.90 east; 36.69 lat; -121.94 lon; 22.50 depth; utmZone: 10 2024-11-06T04:37:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867856.93 2024-11-06T04:37:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:40Z- waiting for 1730867860.93, time is 1730867860.17 2024-11-06T04:37:40Z- AHRS_M2 msg: 1730867860.53 epoch sec; seqNo:323 2024-11-06T04:37:40Z- AHRS_M2 msg: 0.15 phi; -0.26 theta; 5.36 psi 2024-11-06T04:37:40Z- handleDepth()- Depth_Keller msg: 1730867860.53 epoch sec; seqNo:355; depth 22.57 2024-11-06T04:37:40Z- handleDVL() - DVL msg epochMillisec: 1730867860.56 2024-11-06T04:37:40Z- handleDvl() - RDI_Pathfinder msg: 1730867860.56 sec; seqNo:119; #beams:4 2024-11-06T04:37:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 77.70 , 63.78 , 62.81 , 81.26 2024-11-06T04:37:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.05 , -0.10 2024-11-06T04:37:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867860.58 epoch sec; seqNo:355; 4061556.67 north; 594375.90 east; 36.69 lat; -121.94 lon; 22.57 depth; utmZone: 10 2024-11-06T04:37:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867856.93 2024-11-06T04:37:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:40Z- waiting for 1730867860.93, time is 1730867860.58 2024-11-06T04:37:40Z- AHRS_M2 msg: 1730867860.93 epoch sec; seqNo:324 2024-11-06T04:37:40Z- AHRS_M2 msg: 0.15 phi; -0.26 theta; 5.36 psi 2024-11-06T04:37:40Z- handleDepth()- Depth_Keller msg: 1730867860.94 epoch sec; seqNo:356; depth 22.69 2024-11-06T04:37:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867860.95 epoch sec; seqNo:356; 4061557.05 north; 594375.90 east; 36.69 lat; -121.94 lon; 22.69 depth; utmZone: 10 2024-11-06T04:37:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867856.93 2024-11-06T04:37:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:41Z- waiting for 1730867860.93, time is 1730867860.95 2024-11-06T04:37:41Z- LcmTrn::updateTrn() >>>> pose time 1730867860.95, meas time 1730867860.56 <<<< 2024-11-06T04:37:41Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867860.95 2024-11-06T04:37:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867860.56 sec, ping # 0; waiting for INS data more recent than 1730867856.93... 2024-11-06T04:37:41Z- TerrainNav::motionUpdate() - currEstimate = (4061557, 594375.90, 22.69) 2024-11-06T04:37:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:37:41Z- TerrainNav:: SubcloudNIS is 0.38, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:37:41Z- TNavPF::Projecting Measurements 2024-11-06T04:37:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:37:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:37:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:37:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:37:41Z- TNavFilter::4 good beams. 2024-11-06T04:37:41Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:37:41Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:37:41Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:37:41Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:37:41Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:37:41Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:37:41Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:37:41Z- TerrainMapOctree: (vehN, vehE ) = (4061556.83, 594376.14). 2024-11-06T04:37:41Z- TerrainMapOctree: Min Distance = 230.70. 2024-11-06T04:37:41Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:37:41Z- Forcing Subcloud Comparison 2024-11-06T04:37:41Z- Weighting particles with subcloud comparison 2024-11-06T04:37:41Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:41Z- meanExpectedMeasDiff: -0.771472 delta_rms_squared: 3.213238 alpha: 0.993814 2024-11-06T04:37:41Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:41Z- meanExpectedMeasDiff: 1.391114 delta_rms_squared: 8.606979 alpha: 0.997682 2024-11-06T04:37:41Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:41Z- meanExpectedMeasDiff: 1.059378 delta_rms_squared: 2.001783 alpha: 0.990108 2024-11-06T04:37:41Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:41Z- meanExpectedMeasDiff: -0.571338 delta_rms_squared: 4.068951 alpha: 0.995109 2024-11-06T04:37:41Z- Subcloud NIS: 0.337284 2024-11-06T04:37:41Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:37:41Z- TNavPF:: sumWeights = 0.483988 2024-11-06T04:37:41Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:37:41Z- TNavPF::First Term of Map Covariance Matrix is 0.85 2024-11-06T04:37:41Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T04:37:41Z- TerrainNav::Resampling particle filter... 2024-11-06T04:37:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867860.56 sec. 2024-11-06T04:37:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:37:41Z- variances([0], [2], and psi_berg) = 193.932031 89.102491 0.000000 2024-11-06T04:37:41Z- publishEstimatesDecoder() - at 1730867860.95 sec 2024-11-06T04:37:41Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:37:41Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061554.44 594416.62 28.77 2024-11-06T04:37:41Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061505.22 594382.80 32.56 lat/lon: 36.69 -121.94 2024-11-06T04:37:41Z- LcmTrn::publishEstimatesDecoder() - COVAR: 193.93 89.10 1.70 0.00 2024-11-06T04:37:41Z- AHRS_M2 msg: 1730867861.34 epoch sec; seqNo:325 2024-11-06T04:37:41Z- AHRS_M2 msg: 0.15 phi; -0.26 theta; 5.36 psi 2024-11-06T04:37:41Z- handleDepth()- Depth_Keller msg: 1730867861.35 epoch sec; seqNo:357; depth 22.75 2024-11-06T04:37:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867861.37 epoch sec; seqNo:357; 4061557.05 north; 594375.90 east; 36.69 lat; -121.94 lon; 22.75 depth; utmZone: 10 2024-11-06T04:37:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867860.95 2024-11-06T04:37:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:41Z- waiting for 1730867864.95, time is 1730867861.37 2024-11-06T04:37:41Z- AHRS_M2 msg: 1730867861.76 epoch sec; seqNo:326 2024-11-06T04:37:41Z- AHRS_M2 msg: 0.15 phi; -0.26 theta; 5.36 psi 2024-11-06T04:37:41Z- handleDepth()- Depth_Keller msg: 1730867861.78 epoch sec; seqNo:358; depth 22.84 2024-11-06T04:37:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867861.78 epoch sec; seqNo:358; 4061557.42 north; 594374.67 east; 36.69 lat; -121.94 lon; 22.84 depth; utmZone: 10 2024-11-06T04:37:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867860.95 2024-11-06T04:37:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:41Z- waiting for 1730867864.95, time is 1730867861.78 2024-11-06T04:37:42Z- AHRS_M2 msg: 1730867862.14 epoch sec; seqNo:327 2024-11-06T04:37:42Z- AHRS_M2 msg: 0.14 phi; -0.26 theta; 5.36 psi 2024-11-06T04:37:42Z- handleDepth()- Depth_Keller msg: 1730867862.15 epoch sec; seqNo:359; depth 22.93 2024-11-06T04:37:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867862.16 epoch sec; seqNo:359; 4061557.42 north; 594374.67 east; 36.69 lat; -121.94 lon; 22.93 depth; utmZone: 10 2024-11-06T04:37:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867860.95 2024-11-06T04:37:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:42Z- waiting for 1730867864.95, time is 1730867862.16 2024-11-06T04:37:42Z- AHRS_M2 msg: 1730867862.55 epoch sec; seqNo:328 2024-11-06T04:37:42Z- AHRS_M2 msg: 0.14 phi; -0.26 theta; 5.36 psi 2024-11-06T04:37:42Z- handleDepth()- Depth_Keller msg: 1730867862.55 epoch sec; seqNo:360; depth 23.02 2024-11-06T04:37:42Z- handleDVL() - DVL msg epochMillisec: 1730867862.58 2024-11-06T04:37:42Z- handleDvl() - RDI_Pathfinder msg: 1730867862.58 sec; seqNo:120; #beams:4 2024-11-06T04:37:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 76.84 , 62.13 , 61.31 , 85.00 2024-11-06T04:37:42Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , -0.02 , -0.06 2024-11-06T04:37:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867862.59 epoch sec; seqNo:360; 4061557.80 north; 594374.67 east; 36.69 lat; -121.94 lon; 23.02 depth; utmZone: 10 2024-11-06T04:37:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867860.95 2024-11-06T04:37:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:42Z- waiting for 1730867864.95, time is 1730867862.59 2024-11-06T04:37:42Z- AHRS_M2 msg: 1730867862.95 epoch sec; seqNo:329 2024-11-06T04:37:42Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.36 psi 2024-11-06T04:37:42Z- handleDepth()- Depth_Keller msg: 1730867862.97 epoch sec; seqNo:361; depth 23.13 2024-11-06T04:37:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867862.98 epoch sec; seqNo:361; 4061558.18 north; 594374.66 east; 36.69 lat; -121.94 lon; 23.13 depth; utmZone: 10 2024-11-06T04:37:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867860.95 2024-11-06T04:37:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:43Z- waiting for 1730867864.95, time is 1730867862.98 2024-11-06T04:37:43Z- AHRS_M2 msg: 1730867863.35 epoch sec; seqNo:330 2024-11-06T04:37:43Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.36 psi 2024-11-06T04:37:43Z- handleDepth()- Depth_Keller msg: 1730867863.37 epoch sec; seqNo:362; depth 23.21 2024-11-06T04:37:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867863.38 epoch sec; seqNo:362; 4061558.16 north; 594373.44 east; 36.69 lat; -121.94 lon; 23.21 depth; utmZone: 10 2024-11-06T04:37:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867860.95 2024-11-06T04:37:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:43Z- waiting for 1730867864.95, time is 1730867863.38 2024-11-06T04:37:43Z- AHRS_M2 msg: 1730867863.76 epoch sec; seqNo:331 2024-11-06T04:37:43Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.36 psi 2024-11-06T04:37:43Z- handleDepth()- Depth_Keller msg: 1730867863.76 epoch sec; seqNo:363; depth 23.31 2024-11-06T04:37:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867863.77 epoch sec; seqNo:363; 4061558.54 north; 594373.44 east; 36.69 lat; -121.94 lon; 23.31 depth; utmZone: 10 2024-11-06T04:37:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867860.95 2024-11-06T04:37:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:43Z- waiting for 1730867864.95, time is 1730867863.77 2024-11-06T04:37:44Z- AHRS_M2 msg: 1730867864.16 epoch sec; seqNo:332 2024-11-06T04:37:44Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.36 psi 2024-11-06T04:37:44Z- handleDepth()- Depth_Keller msg: 1730867864.17 epoch sec; seqNo:364; depth 23.40 2024-11-06T04:37:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867864.18 epoch sec; seqNo:364; 4061558.54 north; 594373.44 east; 36.70 lat; -121.94 lon; 23.40 depth; utmZone: 10 2024-11-06T04:37:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867860.95 2024-11-06T04:37:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:44Z- waiting for 1730867864.95, time is 1730867864.18 2024-11-06T04:37:44Z- AHRS_M2 msg: 1730867864.57 epoch sec; seqNo:333 2024-11-06T04:37:44Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.36 psi 2024-11-06T04:37:44Z- handleDepth()- Depth_Keller msg: 1730867864.58 epoch sec; seqNo:365; depth 23.49 2024-11-06T04:37:44Z- handleDVL() - DVL msg epochMillisec: 1730867864.60 2024-11-06T04:37:44Z- handleDvl() - RDI_Pathfinder msg: 1730867864.60 sec; seqNo:121; #beams:4 2024-11-06T04:37:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.61 , 62.94 , 61.31 , 78.88 2024-11-06T04:37:44Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.05 , 0.05 2024-11-06T04:37:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867864.62 epoch sec; seqNo:365; 4061558.91 north; 594372.21 east; 36.70 lat; -121.94 lon; 23.49 depth; utmZone: 10 2024-11-06T04:37:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867860.95 2024-11-06T04:37:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:44Z- waiting for 1730867864.95, time is 1730867864.62 2024-11-06T04:37:44Z- AHRS_M2 msg: 1730867864.97 epoch sec; seqNo:334 2024-11-06T04:37:44Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.37 psi 2024-11-06T04:37:44Z- handleDepth()- Depth_Keller msg: 1730867864.97 epoch sec; seqNo:366; depth 23.60 2024-11-06T04:37:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867864.99 epoch sec; seqNo:366; 4061559.29 north; 594372.21 east; 36.70 lat; -121.94 lon; 23.60 depth; utmZone: 10 2024-11-06T04:37:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867860.95 2024-11-06T04:37:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:45Z- waiting for 1730867864.95, time is 1730867864.99 2024-11-06T04:37:45Z- LcmTrn::updateTrn() >>>> pose time 1730867864.99, meas time 1730867864.60 <<<< 2024-11-06T04:37:45Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867864.99 2024-11-06T04:37:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867864.60 sec, ping # 0; waiting for INS data more recent than 1730867860.95... 2024-11-06T04:37:45Z- TerrainNav::motionUpdate() - currEstimate = (4061559, 594372.21, 23.60) 2024-11-06T04:37:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:37:45Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:37:45Z- TNavPF::Projecting Measurements 2024-11-06T04:37:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:37:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:37:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:37:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:37:45Z- TNavFilter::4 good beams. 2024-11-06T04:37:45Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:37:45Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:37:45Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:37:45Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:37:45Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:37:45Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:37:45Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:37:45Z- TerrainMapOctree: (vehN, vehE ) = (4061559.07, 594372.56). 2024-11-06T04:37:45Z- TerrainMapOctree: Min Distance = 226.88. 2024-11-06T04:37:45Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:37:45Z- Forcing Subcloud Comparison 2024-11-06T04:37:45Z- Weighting particles with subcloud comparison 2024-11-06T04:37:45Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:45Z- meanExpectedMeasDiff: -0.914406 delta_rms_squared: 2.633911 alpha: 0.992464 2024-11-06T04:37:45Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:45Z- meanExpectedMeasDiff: -0.039338 delta_rms_squared: 4.104082 alpha: 0.995150 2024-11-06T04:37:45Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:45Z- meanExpectedMeasDiff: 0.490028 delta_rms_squared: 1.458142 alpha: 0.986469 2024-11-06T04:37:45Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:45Z- meanExpectedMeasDiff: -0.935844 delta_rms_squared: 2.871892 alpha: 0.993084 2024-11-06T04:37:45Z- Subcloud NIS: 0.314222 2024-11-06T04:37:45Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:37:45Z- TNavPF:: sumWeights = 0.645128 2024-11-06T04:37:45Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:37:45Z- TNavPF::First Term of Map Covariance Matrix is 0.84 2024-11-06T04:37:45Z- TNavPF::Calculated NIS Value : 0.07 numBeams normalized NIS: 0.02 2024-11-06T04:37:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867864.60 sec. 2024-11-06T04:37:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:37:45Z- variances([0], [2], and psi_berg) = 22.059276 10.711301 0.000000 2024-11-06T04:37:45Z- publishEstimatesDecoder() - at 1730867864.99 sec 2024-11-06T04:37:45Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:37:45Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061503.60 594376.57 33.98 2024-11-06T04:37:45Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061503.69 594376.61 33.82 lat/lon: 36.69 -121.94 2024-11-06T04:37:45Z- LcmTrn::publishEstimatesDecoder() - COVAR: 22.06 10.71 1.03 0.00 2024-11-06T04:37:45Z- AHRS_M2 msg: 1730867865.38 epoch sec; seqNo:335 2024-11-06T04:37:45Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.37 psi 2024-11-06T04:37:45Z- handleDepth()- Depth_Keller msg: 1730867865.38 epoch sec; seqNo:367; depth 23.68 2024-11-06T04:37:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867865.39 epoch sec; seqNo:367; 4061559.29 north; 594372.21 east; 36.70 lat; -121.94 lon; 23.68 depth; utmZone: 10 2024-11-06T04:37:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867864.99 2024-11-06T04:37:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:45Z- waiting for 1730867868.99, time is 1730867865.39 2024-11-06T04:37:45Z- AHRS_M2 msg: 1730867865.79 epoch sec; seqNo:336 2024-11-06T04:37:45Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.37 psi 2024-11-06T04:37:45Z- handleDepth()- Depth_Keller msg: 1730867865.79 epoch sec; seqNo:368; depth 23.78 2024-11-06T04:37:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867865.81 epoch sec; seqNo:368; 4061559.66 north; 594372.21 east; 36.70 lat; -121.94 lon; 23.78 depth; utmZone: 10 2024-11-06T04:37:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867864.99 2024-11-06T04:37:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:45Z- waiting for 1730867868.99, time is 1730867865.81 2024-11-06T04:37:46Z- AHRS_M2 msg: 1730867866.18 epoch sec; seqNo:337 2024-11-06T04:37:46Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.37 psi 2024-11-06T04:37:46Z- handleDepth()- Depth_Keller msg: 1730867866.19 epoch sec; seqNo:369; depth 23.86 2024-11-06T04:37:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867866.20 epoch sec; seqNo:369; 4061559.65 north; 594370.99 east; 36.70 lat; -121.94 lon; 23.86 depth; utmZone: 10 2024-11-06T04:37:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867864.99 2024-11-06T04:37:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:46Z- waiting for 1730867868.99, time is 1730867866.20 2024-11-06T04:37:46Z- AHRS_M2 msg: 1730867866.59 epoch sec; seqNo:338 2024-11-06T04:37:46Z- AHRS_M2 msg: 0.15 phi; -0.25 theta; 5.37 psi 2024-11-06T04:37:46Z- handleDepth()- Depth_Keller msg: 1730867866.59 epoch sec; seqNo:370; depth 23.98 2024-11-06T04:37:46Z- handleDVL() - DVL msg epochMillisec: 1730867866.61 2024-11-06T04:37:46Z- handleDvl() - RDI_Pathfinder msg: 1730867866.61 sec; seqNo:122; #beams:4 2024-11-06T04:37:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.43 , 62.48 , 60.86 , 78.99 2024-11-06T04:37:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.07 , 0.10 2024-11-06T04:37:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867866.64 epoch sec; seqNo:370; 4061560.03 north; 594370.98 east; 36.70 lat; -121.94 lon; 23.98 depth; utmZone: 10 2024-11-06T04:37:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867864.99 2024-11-06T04:37:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:46Z- waiting for 1730867868.99, time is 1730867866.64 2024-11-06T04:37:47Z- AHRS_M2 msg: 1730867866.99 epoch sec; seqNo:339 2024-11-06T04:37:47Z- AHRS_M2 msg: 0.15 phi; -0.25 theta; 5.38 psi 2024-11-06T04:37:47Z- handleDepth()- Depth_Keller msg: 1730867867.00 epoch sec; seqNo:371; depth 24.06 2024-11-06T04:37:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867867.01 epoch sec; seqNo:371; 4061560.03 north; 594370.98 east; 36.70 lat; -121.94 lon; 24.06 depth; utmZone: 10 2024-11-06T04:37:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867864.99 2024-11-06T04:37:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:47Z- waiting for 1730867868.99, time is 1730867867.01 2024-11-06T04:37:47Z- AHRS_M2 msg: 1730867867.40 epoch sec; seqNo:340 2024-11-06T04:37:47Z- AHRS_M2 msg: 0.15 phi; -0.25 theta; 5.38 psi 2024-11-06T04:37:47Z- handleDepth()- Depth_Keller msg: 1730867867.41 epoch sec; seqNo:372; depth 24.16 2024-11-06T04:37:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867867.42 epoch sec; seqNo:372; 4061560.41 north; 594370.98 east; 36.70 lat; -121.94 lon; 24.16 depth; utmZone: 10 2024-11-06T04:37:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867864.99 2024-11-06T04:37:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:47Z- waiting for 1730867868.99, time is 1730867867.42 2024-11-06T04:37:47Z- AHRS_M2 msg: 1730867867.80 epoch sec; seqNo:341 2024-11-06T04:37:47Z- AHRS_M2 msg: 0.15 phi; -0.25 theta; 5.38 psi 2024-11-06T04:37:47Z- handleDepth()- Depth_Keller msg: 1730867867.81 epoch sec; seqNo:373; depth 24.25 2024-11-06T04:37:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867867.84 epoch sec; seqNo:373; 4061560.39 north; 594369.76 east; 36.70 lat; -121.94 lon; 24.25 depth; utmZone: 10 2024-11-06T04:37:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867864.99 2024-11-06T04:37:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:47Z- waiting for 1730867868.99, time is 1730867867.84 2024-11-06T04:37:48Z- AHRS_M2 msg: 1730867868.20 epoch sec; seqNo:342 2024-11-06T04:37:48Z- AHRS_M2 msg: 0.15 phi; -0.25 theta; 5.39 psi 2024-11-06T04:37:48Z- handleDepth()- Depth_Keller msg: 1730867868.21 epoch sec; seqNo:374; depth 24.34 2024-11-06T04:37:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867868.22 epoch sec; seqNo:374; 4061560.77 north; 594369.75 east; 36.70 lat; -121.94 lon; 24.34 depth; utmZone: 10 2024-11-06T04:37:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867864.99 2024-11-06T04:37:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:48Z- waiting for 1730867868.99, time is 1730867868.22 2024-11-06T04:37:48Z- AHRS_M2 msg: 1730867868.61 epoch sec; seqNo:343 2024-11-06T04:37:48Z- AHRS_M2 msg: 0.15 phi; -0.25 theta; 5.39 psi 2024-11-06T04:37:48Z- handleDepth()- Depth_Keller msg: 1730867868.61 epoch sec; seqNo:375; depth 24.42 2024-11-06T04:37:48Z- handleDVL() - DVL msg epochMillisec: 1730867868.63 2024-11-06T04:37:48Z- handleDvl() - RDI_Pathfinder msg: 1730867868.63 sec; seqNo:123; #beams:4 2024-11-06T04:37:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.74 , 62.47 , 60.85 , 78.65 2024-11-06T04:37:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , 0.06 2024-11-06T04:37:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867868.65 epoch sec; seqNo:375; 4061561.15 north; 594369.75 east; 36.70 lat; -121.94 lon; 24.42 depth; utmZone: 10 2024-11-06T04:37:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867864.99 2024-11-06T04:37:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:48Z- waiting for 1730867868.99, time is 1730867868.65 2024-11-06T04:37:49Z- AHRS_M2 msg: 1730867869.01 epoch sec; seqNo:344 2024-11-06T04:37:49Z- AHRS_M2 msg: 0.15 phi; -0.25 theta; 5.39 psi 2024-11-06T04:37:49Z- handleDepth()- Depth_Keller msg: 1730867869.02 epoch sec; seqNo:376; depth 24.52 2024-11-06T04:37:49Z- handleDVL() - DVL msg epochMillisec: 1730867869.02 2024-11-06T04:37:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:37:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:37:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:37:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:37:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:37:49Z- handleDvl() - RDI_Pathfinder msg: 1730867869.02 sec; seqNo:124; #beams:0 2024-11-06T04:37:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:37:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , 0.06 2024-11-06T04:37:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867869.04 epoch sec; seqNo:376; 4061561.15 north; 594369.75 east; 36.70 lat; -121.94 lon; 24.52 depth; utmZone: 10 2024-11-06T04:37:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867864.99 2024-11-06T04:37:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:49Z- waiting for 1730867868.99, time is 1730867869.04 2024-11-06T04:37:49Z- LcmTrn::updateTrn() >>>> pose time 1730867869.04, meas time 1730867869.02 <<<< 2024-11-06T04:37:49Z- LcmTrn::updateTrn() equating pose 1730867869.02 and meas times 1730867869.02 2024-11-06T04:37:49Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867869.02 2024-11-06T04:37:49Z- TerrainNav::motionUpdate() - currEstimate = (4061561, 594369.75, 24.52) 2024-11-06T04:37:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:37:49Z- TerrainNav:: SubcloudNIS is 0.31, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:37:49Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:37:49Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:37:49Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:37:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:37:49Z- variances([0], [2], and psi_berg) = 22.057860 10.712452 0.000000 2024-11-06T04:37:49Z- publishEstimatesDecoder() - at 1730867869.02 sec 2024-11-06T04:37:49Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:37:49Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061505.47 594374.11 34.33 2024-11-06T04:37:49Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061505.56 594374.15 34.73 lat/lon: 36.69 -121.94 2024-11-06T04:37:49Z- LcmTrn::publishEstimatesDecoder() - COVAR: 22.06 10.71 1.28 0.00 2024-11-06T04:37:49Z- AHRS_M2 msg: 1730867869.42 epoch sec; seqNo:345 2024-11-06T04:37:49Z- AHRS_M2 msg: 0.15 phi; -0.25 theta; 5.40 psi 2024-11-06T04:37:49Z- handleDepth()- Depth_Keller msg: 1730867869.43 epoch sec; seqNo:377; depth 24.63 2024-11-06T04:37:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867869.44 epoch sec; seqNo:377; 4061561.53 north; 594369.74 east; 36.70 lat; -121.94 lon; 24.63 depth; utmZone: 10 2024-11-06T04:37:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867869.02 2024-11-06T04:37:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:49Z- waiting for 1730867873.02, time is 1730867869.44 2024-11-06T04:37:49Z- AHRS_M2 msg: 1730867869.82 epoch sec; seqNo:346 2024-11-06T04:37:49Z- AHRS_M2 msg: 0.15 phi; -0.25 theta; 5.40 psi 2024-11-06T04:37:49Z- handleDepth()- Depth_Keller msg: 1730867869.83 epoch sec; seqNo:378; depth 24.70 2024-11-06T04:37:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867869.86 epoch sec; seqNo:378; 4061561.52 north; 594368.52 east; 36.70 lat; -121.94 lon; 24.70 depth; utmZone: 10 2024-11-06T04:37:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867869.02 2024-11-06T04:37:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:49Z- waiting for 1730867873.02, time is 1730867869.86 2024-11-06T04:37:50Z- handleDepth()- Depth_Keller msg: 1730867870.25 epoch sec; seqNo:379; depth 24.78 2024-11-06T04:37:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867870.27 epoch sec; seqNo:379; 4061561.90 north; 594368.52 east; 36.70 lat; -121.94 lon; 24.78 depth; utmZone: 10 2024-11-06T04:37:50Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:37:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867869.02 2024-11-06T04:37:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:50Z- waiting for 1730867873.02, time is 1730867870.27 2024-11-06T04:37:50Z- AHRS_M2 msg: 1730867870.63 epoch sec; seqNo:347 2024-11-06T04:37:50Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.40 psi 2024-11-06T04:37:50Z- handleDepth()- Depth_Keller msg: 1730867870.64 epoch sec; seqNo:380; depth 24.86 2024-11-06T04:37:50Z- handleDVL() - DVL msg epochMillisec: 1730867870.66 2024-11-06T04:37:50Z- handleDvl() - RDI_Pathfinder msg: 1730867870.66 sec; seqNo:125; #beams:4 2024-11-06T04:37:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 75.09 , 62.15 , 60.85 , 78.01 2024-11-06T04:37:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , -0.02 2024-11-06T04:37:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867870.68 epoch sec; seqNo:380; 4061561.90 north; 594368.52 east; 36.70 lat; -121.94 lon; 24.86 depth; utmZone: 10 2024-11-06T04:37:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867869.02 2024-11-06T04:37:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:50Z- waiting for 1730867873.02, time is 1730867870.68 2024-11-06T04:37:51Z- AHRS_M2 msg: 1730867871.03 epoch sec; seqNo:348 2024-11-06T04:37:51Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.41 psi 2024-11-06T04:37:51Z- handleDepth()- Depth_Keller msg: 1730867871.04 epoch sec; seqNo:381; depth 24.97 2024-11-06T04:37:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867871.07 epoch sec; seqNo:381; 4061562.28 north; 594368.52 east; 36.70 lat; -121.94 lon; 24.97 depth; utmZone: 10 2024-11-06T04:37:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867869.02 2024-11-06T04:37:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:51Z- waiting for 1730867873.02, time is 1730867871.07 2024-11-06T04:37:51Z- handleDepth()- Depth_Keller msg: 1730867871.46 epoch sec; seqNo:382; depth 25.05 2024-11-06T04:37:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867871.47 epoch sec; seqNo:382; 4061562.26 north; 594367.30 east; 36.70 lat; -121.94 lon; 25.05 depth; utmZone: 10 2024-11-06T04:37:51Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:37:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867869.02 2024-11-06T04:37:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:51Z- waiting for 1730867873.02, time is 1730867871.47 2024-11-06T04:37:51Z- AHRS_M2 msg: 1730867871.85 epoch sec; seqNo:349 2024-11-06T04:37:51Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.41 psi 2024-11-06T04:37:51Z- handleDepth()- Depth_Keller msg: 1730867871.86 epoch sec; seqNo:383; depth 25.15 2024-11-06T04:37:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867871.88 epoch sec; seqNo:383; 4061562.64 north; 594367.29 east; 36.70 lat; -121.94 lon; 25.15 depth; utmZone: 10 2024-11-06T04:37:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867869.02 2024-11-06T04:37:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:52Z- waiting for 1730867873.02, time is 1730867871.88 2024-11-06T04:37:52Z- AHRS_M2 msg: 1730867872.25 epoch sec; seqNo:350 2024-11-06T04:37:52Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.41 psi 2024-11-06T04:37:52Z- handleDepth()- Depth_Keller msg: 1730867872.25 epoch sec; seqNo:384; depth 25.21 2024-11-06T04:37:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867872.26 epoch sec; seqNo:384; 4061562.64 north; 594367.29 east; 36.70 lat; -121.94 lon; 25.21 depth; utmZone: 10 2024-11-06T04:37:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867869.02 2024-11-06T04:37:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:52Z- waiting for 1730867873.02, time is 1730867872.26 2024-11-06T04:37:52Z- AHRS_M2 msg: 1730867872.65 epoch sec; seqNo:351 2024-11-06T04:37:52Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.41 psi 2024-11-06T04:37:52Z- handleDepth()- Depth_Keller msg: 1730867872.66 epoch sec; seqNo:385; depth 25.30 2024-11-06T04:37:52Z- handleDVL() - DVL msg epochMillisec: 1730867872.71 2024-11-06T04:37:52Z- handleDvl() - RDI_Pathfinder msg: 1730867872.71 sec; seqNo:126; #beams:4 2024-11-06T04:37:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.44 , 62.14 , 60.20 , 77.35 2024-11-06T04:37:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.08 , -0.05 2024-11-06T04:37:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867872.73 epoch sec; seqNo:385; 4061563.02 north; 594367.29 east; 36.70 lat; -121.94 lon; 25.30 depth; utmZone: 10 2024-11-06T04:37:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867869.02 2024-11-06T04:37:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:52Z- waiting for 1730867873.02, time is 1730867872.73 2024-11-06T04:37:53Z- AHRS_M2 msg: 1730867873.06 epoch sec; seqNo:352 2024-11-06T04:37:53Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.41 psi 2024-11-06T04:37:53Z- handleDepth()- Depth_Keller msg: 1730867873.06 epoch sec; seqNo:386; depth 25.41 2024-11-06T04:37:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867873.08 epoch sec; seqNo:386; 4061563.40 north; 594367.28 east; 36.70 lat; -121.94 lon; 25.41 depth; utmZone: 10 2024-11-06T04:37:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867869.02 2024-11-06T04:37:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:53Z- waiting for 1730867873.02, time is 1730867873.08 2024-11-06T04:37:53Z- LcmTrn::updateTrn() >>>> pose time 1730867873.08, meas time 1730867872.71 <<<< 2024-11-06T04:37:53Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867873.08 2024-11-06T04:37:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867872.71 sec, ping # 0; waiting for INS data more recent than 1730867869.02... 2024-11-06T04:37:53Z- TerrainNav::motionUpdate() - currEstimate = (4061563, 594367.28, 25.41) 2024-11-06T04:37:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:37:53Z- TerrainNav:: SubcloudNIS is 0.31, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:37:53Z- TNavPF::Projecting Measurements 2024-11-06T04:37:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:37:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:37:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:37:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:37:53Z- TNavFilter::4 good beams. 2024-11-06T04:37:53Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:37:53Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:37:53Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:37:53Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:37:53Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:37:53Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:37:53Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:37:53Z- TerrainMapOctree: (vehN, vehE ) = (4061563.19, 594367.51). 2024-11-06T04:37:53Z- TerrainMapOctree: Min Distance = 220.69. 2024-11-06T04:37:53Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:37:53Z- Forcing Subcloud Comparison 2024-11-06T04:37:53Z- Weighting particles with subcloud comparison 2024-11-06T04:37:53Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:53Z- meanExpectedMeasDiff: -0.558021 delta_rms_squared: 2.565927 alpha: 0.992266 2024-11-06T04:37:53Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:53Z- meanExpectedMeasDiff: 0.968271 delta_rms_squared: 2.052781 alpha: 0.990351 2024-11-06T04:37:53Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:53Z- meanExpectedMeasDiff: 1.365058 delta_rms_squared: 1.573170 alpha: 0.987446 2024-11-06T04:37:53Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:53Z- meanExpectedMeasDiff: -0.519915 delta_rms_squared: 2.860158 alpha: 0.993056 2024-11-06T04:37:53Z- Subcloud NIS: 0.307177 2024-11-06T04:37:53Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:37:53Z- TNavPF:: sumWeights = 0.592639 2024-11-06T04:37:53Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:37:53Z- TNavPF::First Term of Map Covariance Matrix is 0.84 2024-11-06T04:37:53Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T04:37:53Z- TerrainNav::Resampling particle filter... 2024-11-06T04:37:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867872.71 sec. 2024-11-06T04:37:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:37:53Z- variances([0], [2], and psi_berg) = 0.978829 0.533869 0.000000 2024-11-06T04:37:53Z- publishEstimatesDecoder() - at 1730867873.08 sec 2024-11-06T04:37:53Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:37:53Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061557.61 594413.45 30.66 2024-11-06T04:37:53Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061507.39 594371.40 35.51 lat/lon: 36.69 -121.94 2024-11-06T04:37:53Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.98 0.53 1.00 0.00 2024-11-06T04:37:53Z- AHRS_M2 msg: 1730867873.47 epoch sec; seqNo:353 2024-11-06T04:37:53Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.41 psi 2024-11-06T04:37:53Z- handleDepth()- Depth_Keller msg: 1730867873.47 epoch sec; seqNo:387; depth 25.48 2024-11-06T04:37:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867873.48 epoch sec; seqNo:387; 4061563.39 north; 594366.06 east; 36.70 lat; -121.94 lon; 25.48 depth; utmZone: 10 2024-11-06T04:37:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867873.08 2024-11-06T04:37:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:53Z- waiting for 1730867877.08, time is 1730867873.48 2024-11-06T04:37:53Z- AHRS_M2 msg: 1730867873.86 epoch sec; seqNo:354 2024-11-06T04:37:53Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.41 psi 2024-11-06T04:37:53Z- handleDepth()- Depth_Keller msg: 1730867873.87 epoch sec; seqNo:388; depth 25.57 2024-11-06T04:37:53Z- handleDVL() - DVL msg epochMillisec: 1730867873.87 2024-11-06T04:37:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:37:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:37:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:37:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:37:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:37:53Z- handleDvl() - RDI_Pathfinder msg: 1730867873.87 sec; seqNo:127; #beams:0 2024-11-06T04:37:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:37:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.08 , -0.05 2024-11-06T04:37:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867873.88 epoch sec; seqNo:388; 4061563.76 north; 594366.06 east; 36.70 lat; -121.94 lon; 25.57 depth; utmZone: 10 2024-11-06T04:37:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867873.08 2024-11-06T04:37:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:54Z- waiting for 1730867877.08, time is 1730867873.88 2024-11-06T04:37:54Z- AHRS_M2 msg: 1730867874.26 epoch sec; seqNo:355 2024-11-06T04:37:54Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.41 psi 2024-11-06T04:37:54Z- handleDepth()- Depth_Keller msg: 1730867874.28 epoch sec; seqNo:389; depth 25.65 2024-11-06T04:37:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867874.28 epoch sec; seqNo:389; 4061563.76 north; 594366.06 east; 36.70 lat; -121.94 lon; 25.65 depth; utmZone: 10 2024-11-06T04:37:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867873.08 2024-11-06T04:37:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:54Z- waiting for 1730867877.08, time is 1730867874.28 2024-11-06T04:37:54Z- AHRS_M2 msg: 1730867874.67 epoch sec; seqNo:356 2024-11-06T04:37:54Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.42 psi 2024-11-06T04:37:54Z- handleDepth()- Depth_Keller msg: 1730867874.68 epoch sec; seqNo:390; depth 25.74 2024-11-06T04:37:54Z- handleDVL() - DVL msg epochMillisec: 1730867874.71 2024-11-06T04:37:54Z- handleDvl() - RDI_Pathfinder msg: 1730867874.71 sec; seqNo:128; #beams:4 2024-11-06T04:37:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 74.12 , 61.49 , 59.55 , 76.38 2024-11-06T04:37:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.08 , -0.05 2024-11-06T04:37:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867874.72 epoch sec; seqNo:390; 4061564.14 north; 594366.05 east; 36.70 lat; -121.94 lon; 25.74 depth; utmZone: 10 2024-11-06T04:37:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867873.08 2024-11-06T04:37:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:54Z- waiting for 1730867877.08, time is 1730867874.72 2024-11-06T04:37:55Z- AHRS_M2 msg: 1730867875.07 epoch sec; seqNo:357 2024-11-06T04:37:55Z- AHRS_M2 msg: 0.13 phi; -0.24 theta; 5.42 psi 2024-11-06T04:37:55Z- handleDepth()- Depth_Keller msg: 1730867875.08 epoch sec; seqNo:391; depth 25.82 2024-11-06T04:37:55Z- handleDVL() - DVL msg epochMillisec: 1730867875.08 2024-11-06T04:37:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:37:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:37:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:37:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:37:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:37:55Z- handleDvl() - RDI_Pathfinder msg: 1730867875.08 sec; seqNo:129; #beams:0 2024-11-06T04:37:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:37:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.08 , -0.05 2024-11-06T04:37:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867875.09 epoch sec; seqNo:391; 4061564.13 north; 594364.83 east; 36.70 lat; -121.94 lon; 25.82 depth; utmZone: 10 2024-11-06T04:37:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867873.08 2024-11-06T04:37:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:55Z- waiting for 1730867877.08, time is 1730867875.09 2024-11-06T04:37:55Z- AHRS_M2 msg: 1730867875.48 epoch sec; seqNo:358 2024-11-06T04:37:55Z- AHRS_M2 msg: 0.13 phi; -0.24 theta; 5.42 psi 2024-11-06T04:37:55Z- handleDepth()- Depth_Keller msg: 1730867875.48 epoch sec; seqNo:392; depth 25.91 2024-11-06T04:37:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867875.50 epoch sec; seqNo:392; 4061564.51 north; 594364.83 east; 36.70 lat; -121.94 lon; 25.91 depth; utmZone: 10 2024-11-06T04:37:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867873.08 2024-11-06T04:37:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:55Z- waiting for 1730867877.08, time is 1730867875.50 2024-11-06T04:37:55Z- AHRS_M2 msg: 1730867875.88 epoch sec; seqNo:359 2024-11-06T04:37:55Z- AHRS_M2 msg: 0.13 phi; -0.24 theta; 5.42 psi 2024-11-06T04:37:55Z- handleDepth()- Depth_Keller msg: 1730867875.88 epoch sec; seqNo:393; depth 25.98 2024-11-06T04:37:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867875.89 epoch sec; seqNo:393; 4061564.89 north; 594364.83 east; 36.70 lat; -121.94 lon; 25.98 depth; utmZone: 10 2024-11-06T04:37:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867873.08 2024-11-06T04:37:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:56Z- waiting for 1730867877.08, time is 1730867875.89 2024-11-06T04:37:56Z- AHRS_M2 msg: 1730867876.29 epoch sec; seqNo:360 2024-11-06T04:37:56Z- AHRS_M2 msg: 0.13 phi; -0.24 theta; 5.42 psi 2024-11-06T04:37:56Z- handleDepth()- Depth_Keller msg: 1730867876.30 epoch sec; seqNo:394; depth 26.08 2024-11-06T04:37:56Z- handleDVL() - DVL msg epochMillisec: 1730867876.30 2024-11-06T04:37:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:37:56Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:37:56Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:37:56Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:37:56Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:37:56Z- handleDvl() - RDI_Pathfinder msg: 1730867876.30 sec; seqNo:130; #beams:0 2024-11-06T04:37:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:37:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.08 , -0.05 2024-11-06T04:37:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867876.31 epoch sec; seqNo:394; 4061564.89 north; 594364.83 east; 36.70 lat; -121.94 lon; 26.08 depth; utmZone: 10 2024-11-06T04:37:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867873.08 2024-11-06T04:37:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:56Z- waiting for 1730867877.08, time is 1730867876.31 2024-11-06T04:37:56Z- AHRS_M2 msg: 1730867876.69 epoch sec; seqNo:361 2024-11-06T04:37:56Z- AHRS_M2 msg: 0.13 phi; -0.24 theta; 5.42 psi 2024-11-06T04:37:56Z- handleDepth()- Depth_Keller msg: 1730867876.69 epoch sec; seqNo:395; depth 26.15 2024-11-06T04:37:56Z- handleDVL() - DVL msg epochMillisec: 1730867876.73 2024-11-06T04:37:56Z- handleDvl() - RDI_Pathfinder msg: 1730867876.73 sec; seqNo:131; #beams:4 2024-11-06T04:37:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 73.47 , 60.85 , 59.55 , 76.06 2024-11-06T04:37:56Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.06 , -0.07 2024-11-06T04:37:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867876.75 epoch sec; seqNo:395; 4061565.25 north; 594363.60 east; 36.70 lat; -121.94 lon; 26.15 depth; utmZone: 10 2024-11-06T04:37:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867873.08 2024-11-06T04:37:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:56Z- waiting for 1730867877.08, time is 1730867876.75 2024-11-06T04:37:57Z- AHRS_M2 msg: 1730867877.09 epoch sec; seqNo:362 2024-11-06T04:37:57Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.42 psi 2024-11-06T04:37:57Z- handleDepth()- Depth_Keller msg: 1730867877.10 epoch sec; seqNo:396; depth 26.26 2024-11-06T04:37:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867877.11 epoch sec; seqNo:396; 4061565.25 north; 594363.60 east; 36.70 lat; -121.94 lon; 26.26 depth; utmZone: 10 2024-11-06T04:37:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867873.08 2024-11-06T04:37:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:57Z- waiting for 1730867877.08, time is 1730867877.11 2024-11-06T04:37:57Z- LcmTrn::updateTrn() >>>> pose time 1730867877.11, meas time 1730867876.73 <<<< 2024-11-06T04:37:57Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867877.11 2024-11-06T04:37:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867876.73 sec, ping # 0; waiting for INS data more recent than 1730867873.08... 2024-11-06T04:37:57Z- TerrainNav::motionUpdate() - currEstimate = (4061565, 594363.60, 26.26) 2024-11-06T04:37:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:37:57Z- TerrainNav:: SubcloudNIS is 0.31, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:37:57Z- TNavPF::Projecting Measurements 2024-11-06T04:37:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:37:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:37:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:37:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:37:57Z- TNavFilter::4 good beams. 2024-11-06T04:37:57Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:37:57Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:37:57Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:37:57Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:37:57Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:37:57Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:37:57Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:37:57Z- TerrainMapOctree: (vehN, vehE ) = (4061565.08, 594363.94). 2024-11-06T04:37:57Z- TerrainMapOctree: Min Distance = 217.21. 2024-11-06T04:37:57Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:37:57Z- Forcing Subcloud Comparison 2024-11-06T04:37:57Z- Weighting particles with subcloud comparison 2024-11-06T04:37:57Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:57Z- meanExpectedMeasDiff: 0.329561 delta_rms_squared: 2.081770 alpha: 0.990484 2024-11-06T04:37:57Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:57Z- meanExpectedMeasDiff: 0.310717 delta_rms_squared: 1.535022 alpha: 0.987138 2024-11-06T04:37:57Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:57Z- meanExpectedMeasDiff: 1.348478 delta_rms_squared: 1.362492 alpha: 0.985533 2024-11-06T04:37:57Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:37:57Z- meanExpectedMeasDiff: 0.282671 delta_rms_squared: 2.402632 alpha: 0.991745 2024-11-06T04:37:57Z- Subcloud NIS: 0.286418 2024-11-06T04:37:57Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:37:57Z- TNavPF:: sumWeights = 0.672155 2024-11-06T04:37:57Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:37:57Z- TNavPF::First Term of Map Covariance Matrix is 0.79 2024-11-06T04:37:57Z- TNavPF::Calculated NIS Value : 0.09 numBeams normalized NIS: 0.02 2024-11-06T04:37:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867876.73 sec. 2024-11-06T04:37:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:37:57Z- variances([0], [2], and psi_berg) = 0.220211 0.134928 0.000000 2024-11-06T04:37:57Z- publishEstimatesDecoder() - at 1730867877.11 sec 2024-11-06T04:37:57Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:37:57Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061509.21 594367.72 35.91 2024-11-06T04:37:57Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061509.23 594367.70 36.16 lat/lon: 36.69 -121.94 2024-11-06T04:37:57Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.13 0.95 0.00 2024-11-06T04:37:57Z- AHRS_M2 msg: 1730867877.50 epoch sec; seqNo:363 2024-11-06T04:37:57Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.42 psi 2024-11-06T04:37:57Z- handleDepth()- Depth_Keller msg: 1730867877.53 epoch sec; seqNo:397; depth 26.33 2024-11-06T04:37:57Z- handleDVL() - DVL msg epochMillisec: 1730867877.54 2024-11-06T04:37:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:37:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:37:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:37:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:37:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:37:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:37:57Z- handleDvl() - RDI_Pathfinder msg: 1730867877.54 sec; seqNo:132; #beams:0 2024-11-06T04:37:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:37:57Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.06 , -0.07 2024-11-06T04:37:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867877.56 epoch sec; seqNo:397; 4061565.63 north; 594363.60 east; 36.70 lat; -121.94 lon; 26.33 depth; utmZone: 10 2024-11-06T04:37:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867877.11 2024-11-06T04:37:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:57Z- waiting for 1730867881.11, time is 1730867877.56 2024-11-06T04:37:57Z- AHRS_M2 msg: 1730867877.91 epoch sec; seqNo:364 2024-11-06T04:37:57Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.42 psi 2024-11-06T04:37:57Z- handleDepth()- Depth_Keller msg: 1730867877.91 epoch sec; seqNo:398; depth 26.41 2024-11-06T04:37:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867877.94 epoch sec; seqNo:398; 4061566.01 north; 594363.59 east; 36.70 lat; -121.94 lon; 26.41 depth; utmZone: 10 2024-11-06T04:37:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867877.11 2024-11-06T04:37:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:58Z- waiting for 1730867881.11, time is 1730867877.94 2024-11-06T04:37:58Z- AHRS_M2 msg: 1730867878.31 epoch sec; seqNo:365 2024-11-06T04:37:58Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.41 psi 2024-11-06T04:37:58Z- handleDepth()- Depth_Keller msg: 1730867878.31 epoch sec; seqNo:399; depth 26.51 2024-11-06T04:37:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867878.33 epoch sec; seqNo:399; 4061566.00 north; 594362.37 east; 36.70 lat; -121.94 lon; 26.51 depth; utmZone: 10 2024-11-06T04:37:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867877.11 2024-11-06T04:37:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:58Z- waiting for 1730867881.11, time is 1730867878.33 2024-11-06T04:37:58Z- AHRS_M2 msg: 1730867878.71 epoch sec; seqNo:366 2024-11-06T04:37:58Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.41 psi 2024-11-06T04:37:58Z- handleDepth()- Depth_Keller msg: 1730867878.71 epoch sec; seqNo:400; depth 26.59 2024-11-06T04:37:58Z- handleDVL() - DVL msg epochMillisec: 1730867878.74 2024-11-06T04:37:58Z- handleDvl() - RDI_Pathfinder msg: 1730867878.74 sec; seqNo:133; #beams:4 2024-11-06T04:37:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 72.50 , 60.52 , 58.90 , 75.09 2024-11-06T04:37:58Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , -0.04 , 0.04 2024-11-06T04:37:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867878.75 epoch sec; seqNo:400; 4061566.38 north; 594362.37 east; 36.70 lat; -121.94 lon; 26.59 depth; utmZone: 10 2024-11-06T04:37:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:37:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867877.11 2024-11-06T04:37:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:58Z- waiting for 1730867881.11, time is 1730867878.75 2024-11-06T04:37:59Z- AHRS_M2 msg: 1730867879.12 epoch sec; seqNo:367 2024-11-06T04:37:59Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.40 psi 2024-11-06T04:37:59Z- handleDepth()- Depth_Keller msg: 1730867879.13 epoch sec; seqNo:401; depth 26.69 2024-11-06T04:37:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867879.15 epoch sec; seqNo:401; 4061566.75 north; 594362.36 east; 36.70 lat; -121.94 lon; 26.69 depth; utmZone: 10 2024-11-06T04:37:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867877.11 2024-11-06T04:37:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:59Z- waiting for 1730867881.11, time is 1730867879.15 2024-11-06T04:37:59Z- AHRS_M2 msg: 1730867879.52 epoch sec; seqNo:368 2024-11-06T04:37:59Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.40 psi 2024-11-06T04:37:59Z- handleDepth()- Depth_Keller msg: 1730867879.52 epoch sec; seqNo:402; depth 26.76 2024-11-06T04:37:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867879.53 epoch sec; seqNo:402; 4061566.75 north; 594362.36 east; 36.70 lat; -121.94 lon; 26.76 depth; utmZone: 10 2024-11-06T04:37:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:37:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867877.11 2024-11-06T04:37:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:37:59Z- waiting for 1730867881.11, time is 1730867879.53 2024-11-06T04:37:59Z- AHRS_M2 msg: 1730867879.92 epoch sec; seqNo:369 2024-11-06T04:37:59Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.39 psi 2024-11-06T04:37:59Z- handleDepth()- Depth_Keller msg: 1730867879.92 epoch sec; seqNo:403; depth 26.85 2024-11-06T04:37:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867879.94 epoch sec; seqNo:403; 4061567.12 north; 594361.14 east; 36.70 lat; -121.94 lon; 26.85 depth; utmZone: 10 2024-11-06T04:38:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867877.11 2024-11-06T04:38:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:00Z- waiting for 1730867881.11, time is 1730867879.94 2024-11-06T04:38:00Z- AHRS_M2 msg: 1730867880.33 epoch sec; seqNo:370 2024-11-06T04:38:00Z- AHRS_M2 msg: 0.12 phi; -0.23 theta; 5.39 psi 2024-11-06T04:38:00Z- handleDepth()- Depth_Keller msg: 1730867880.33 epoch sec; seqNo:404; depth 26.93 2024-11-06T04:38:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867880.37 epoch sec; seqNo:404; 4061567.50 north; 594361.14 east; 36.70 lat; -121.94 lon; 26.93 depth; utmZone: 10 2024-11-06T04:38:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867877.11 2024-11-06T04:38:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:00Z- waiting for 1730867881.11, time is 1730867880.37 2024-11-06T04:38:00Z- AHRS_M2 msg: 1730867880.73 epoch sec; seqNo:371 2024-11-06T04:38:00Z- AHRS_M2 msg: 0.12 phi; -0.23 theta; 5.39 psi 2024-11-06T04:38:00Z- handleDepth()- Depth_Keller msg: 1730867880.73 epoch sec; seqNo:405; depth 27.03 2024-11-06T04:38:00Z- handleDVL() - DVL msg epochMillisec: 1730867880.77 2024-11-06T04:38:00Z- handleDvl() - RDI_Pathfinder msg: 1730867880.77 sec; seqNo:134; #beams:4 2024-11-06T04:38:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 71.13 , 60.40 , 58.87 , 73.89 2024-11-06T04:38:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.06 , 0.15 2024-11-06T04:38:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867880.78 epoch sec; seqNo:405; 4061567.50 north; 594361.14 east; 36.70 lat; -121.94 lon; 27.03 depth; utmZone: 10 2024-11-06T04:38:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867877.11 2024-11-06T04:38:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:00Z- waiting for 1730867881.11, time is 1730867880.78 2024-11-06T04:38:01Z- AHRS_M2 msg: 1730867881.14 epoch sec; seqNo:372 2024-11-06T04:38:01Z- AHRS_M2 msg: 0.12 phi; -0.23 theta; 5.39 psi 2024-11-06T04:38:01Z- handleDepth()- Depth_Keller msg: 1730867881.15 epoch sec; seqNo:406; depth 27.13 2024-11-06T04:38:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867881.17 epoch sec; seqNo:406; 4061567.86 north; 594359.91 east; 36.70 lat; -121.94 lon; 27.13 depth; utmZone: 10 2024-11-06T04:38:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867877.11 2024-11-06T04:38:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:01Z- waiting for 1730867881.11, time is 1730867881.17 2024-11-06T04:38:01Z- LcmTrn::updateTrn() >>>> pose time 1730867881.17, meas time 1730867880.77 <<<< 2024-11-06T04:38:01Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867881.17 2024-11-06T04:38:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867880.77 sec, ping # 0; waiting for INS data more recent than 1730867877.11... 2024-11-06T04:38:01Z- TerrainNav::motionUpdate() - currEstimate = (4061568, 594359.91, 27.13) 2024-11-06T04:38:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:38:01Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:38:01Z- TNavPF::Projecting Measurements 2024-11-06T04:38:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:38:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:38:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:38:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:38:01Z- TNavFilter::4 good beams. 2024-11-06T04:38:01Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:38:01Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:38:01Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:38:01Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:38:01Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:38:01Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:38:01Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:38:01Z- TerrainMapOctree: (vehN, vehE ) = (4061567.61, 594360.27). 2024-11-06T04:38:01Z- TerrainMapOctree: Min Distance = 213.14. 2024-11-06T04:38:01Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:38:01Z- Forcing Subcloud Comparison 2024-11-06T04:38:01Z- Weighting particles with subcloud comparison 2024-11-06T04:38:01Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:01Z- meanExpectedMeasDiff: -0.654377 delta_rms_squared: 1.978490 alpha: 0.989992 2024-11-06T04:38:01Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:01Z- meanExpectedMeasDiff: 0.390615 delta_rms_squared: 1.486574 alpha: 0.986725 2024-11-06T04:38:01Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:01Z- meanExpectedMeasDiff: 1.342139 delta_rms_squared: 1.415361 alpha: 0.986066 2024-11-06T04:38:01Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:01Z- meanExpectedMeasDiff: -0.734587 delta_rms_squared: 2.301813 alpha: 0.991386 2024-11-06T04:38:01Z- Subcloud NIS: 0.280433 2024-11-06T04:38:01Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:38:01Z- TNavPF:: sumWeights = 0.622708 2024-11-06T04:38:01Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:38:01Z- TNavPF::First Term of Map Covariance Matrix is 0.73 2024-11-06T04:38:01Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T04:38:01Z- TerrainNav::Resampling particle filter... 2024-11-06T04:38:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867880.77 sec. 2024-11-06T04:38:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:38:01Z- variances([0], [2], and psi_berg) = 0.036645 0.033820 0.000000 2024-11-06T04:38:01Z- publishEstimatesDecoder() - at 1730867881.17 sec 2024-11-06T04:38:01Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:38:01Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061511.73 594363.94 36.17 2024-11-06T04:38:01Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061511.84 594364.01 36.98 lat/lon: 36.69 -121.94 2024-11-06T04:38:01Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.04 0.03 0.92 0.00 2024-11-06T04:38:01Z- AHRS_M2 msg: 1730867881.54 epoch sec; seqNo:373 2024-11-06T04:38:01Z- AHRS_M2 msg: 0.12 phi; -0.23 theta; 5.39 psi 2024-11-06T04:38:01Z- handleDepth()- Depth_Keller msg: 1730867881.55 epoch sec; seqNo:407; depth 27.19 2024-11-06T04:38:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867881.57 epoch sec; seqNo:407; 4061567.86 north; 594359.91 east; 36.70 lat; -121.94 lon; 27.19 depth; utmZone: 10 2024-11-06T04:38:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867881.17 2024-11-06T04:38:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:01Z- waiting for 1730867885.17, time is 1730867881.57 2024-11-06T04:38:01Z- AHRS_M2 msg: 1730867881.94 epoch sec; seqNo:374 2024-11-06T04:38:01Z- AHRS_M2 msg: 0.12 phi; -0.23 theta; 5.40 psi 2024-11-06T04:38:01Z- handleDepth()- Depth_Keller msg: 1730867881.95 epoch sec; seqNo:408; depth 27.27 2024-11-06T04:38:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867881.97 epoch sec; seqNo:408; 4061568.24 north; 594359.91 east; 36.70 lat; -121.94 lon; 27.27 depth; utmZone: 10 2024-11-06T04:38:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867881.17 2024-11-06T04:38:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:02Z- waiting for 1730867885.17, time is 1730867881.97 2024-11-06T04:38:02Z- AHRS_M2 msg: 1730867882.36 epoch sec; seqNo:375 2024-11-06T04:38:02Z- AHRS_M2 msg: 0.12 phi; -0.23 theta; 5.40 psi 2024-11-06T04:38:02Z- handleDepth()- Depth_Keller msg: 1730867882.41 epoch sec; seqNo:409; depth 27.35 2024-11-06T04:38:02Z- handleDVL() - DVL msg epochMillisec: 1730867882.44 2024-11-06T04:38:02Z- handleDvl() - RDI_Pathfinder msg: 1730867882.44 sec; seqNo:135; #beams:4 2024-11-06T04:38:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.82 , 60.40 , 58.56 , 74.19 2024-11-06T04:38:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , 0.04 2024-11-06T04:38:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867882.45 epoch sec; seqNo:409; 4061568.24 north; 594359.91 east; 36.70 lat; -121.94 lon; 27.35 depth; utmZone: 10 2024-11-06T04:38:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867881.17 2024-11-06T04:38:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:02Z- waiting for 1730867885.17, time is 1730867882.45 2024-11-06T04:38:02Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:38:02Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:38:02Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:38:02Z- AHRS_M2 msg: 1730867882.74 epoch sec; seqNo:376 2024-11-06T04:38:02Z- AHRS_M2 msg: 0.12 phi; -0.23 theta; 5.40 psi 2024-11-06T04:38:02Z- handleDepth()- Depth_Keller msg: 1730867882.78 epoch sec; seqNo:410; depth 27.45 2024-11-06T04:38:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867882.80 epoch sec; seqNo:410; 4061568.62 north; 594359.90 east; 36.70 lat; -121.94 lon; 27.45 depth; utmZone: 10 2024-11-06T04:38:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867881.17 2024-11-06T04:38:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:02Z- waiting for 1730867885.17, time is 1730867882.80 2024-11-06T04:38:03Z- AHRS_M2 msg: 1730867883.16 epoch sec; seqNo:377 2024-11-06T04:38:03Z- AHRS_M2 msg: 0.12 phi; -0.23 theta; 5.40 psi 2024-11-06T04:38:03Z- handleDepth()- Depth_Keller msg: 1730867883.17 epoch sec; seqNo:411; depth 27.51 2024-11-06T04:38:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867883.18 epoch sec; seqNo:411; 4061568.61 north; 594358.68 east; 36.70 lat; -121.94 lon; 27.51 depth; utmZone: 10 2024-11-06T04:38:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867881.17 2024-11-06T04:38:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:03Z- waiting for 1730867885.17, time is 1730867883.18 2024-11-06T04:38:03Z- AHRS_M2 msg: 1730867883.56 epoch sec; seqNo:378 2024-11-06T04:38:03Z- AHRS_M2 msg: 0.12 phi; -0.23 theta; 5.40 psi 2024-11-06T04:38:03Z- handleDepth()- Depth_Keller msg: 1730867883.57 epoch sec; seqNo:412; depth 27.59 2024-11-06T04:38:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867883.59 epoch sec; seqNo:412; 4061568.99 north; 594358.68 east; 36.70 lat; -121.94 lon; 27.59 depth; utmZone: 10 2024-11-06T04:38:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867881.17 2024-11-06T04:38:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:03Z- waiting for 1730867885.17, time is 1730867883.59 2024-11-06T04:38:03Z- AHRS_M2 msg: 1730867883.96 epoch sec; seqNo:379 2024-11-06T04:38:03Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.40 psi 2024-11-06T04:38:03Z- handleDepth()- Depth_Keller msg: 1730867883.99 epoch sec; seqNo:413; depth 27.67 2024-11-06T04:38:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867884.00 epoch sec; seqNo:413; 4061568.99 north; 594358.68 east; 36.70 lat; -121.94 lon; 27.67 depth; utmZone: 10 2024-11-06T04:38:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867881.17 2024-11-06T04:38:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:04Z- waiting for 1730867885.17, time is 1730867884.00 2024-11-06T04:38:04Z- AHRS_M2 msg: 1730867884.42 epoch sec; seqNo:380 2024-11-06T04:38:04Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.40 psi 2024-11-06T04:38:04Z- handleDepth()- Depth_Keller msg: 1730867884.42 epoch sec; seqNo:414; depth 27.75 2024-11-06T04:38:04Z- handleDVL() - DVL msg epochMillisec: 1730867884.45 2024-11-06T04:38:04Z- handleDvl() - RDI_Pathfinder msg: 1730867884.45 sec; seqNo:136; #beams:4 2024-11-06T04:38:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.82 , 60.09 , 58.25 , 74.19 2024-11-06T04:38:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , -0.06 2024-11-06T04:38:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867884.46 epoch sec; seqNo:414; 4061569.37 north; 594358.67 east; 36.70 lat; -121.94 lon; 27.75 depth; utmZone: 10 2024-11-06T04:38:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867881.17 2024-11-06T04:38:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:04Z- waiting for 1730867885.17, time is 1730867884.46 2024-11-06T04:38:04Z- AHRS_M2 msg: 1730867884.77 epoch sec; seqNo:381 2024-11-06T04:38:04Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.40 psi 2024-11-06T04:38:04Z- handleDepth()- Depth_Keller msg: 1730867884.78 epoch sec; seqNo:415; depth 27.84 2024-11-06T04:38:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867884.79 epoch sec; seqNo:415; 4061569.35 north; 594357.45 east; 36.70 lat; -121.94 lon; 27.84 depth; utmZone: 10 2024-11-06T04:38:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867881.17 2024-11-06T04:38:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:04Z- waiting for 1730867885.17, time is 1730867884.79 2024-11-06T04:38:05Z- AHRS_M2 msg: 1730867885.21 epoch sec; seqNo:382 2024-11-06T04:38:05Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.40 psi 2024-11-06T04:38:05Z- handleDepth()- Depth_Keller msg: 1730867885.21 epoch sec; seqNo:416; depth 27.90 2024-11-06T04:38:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867885.22 epoch sec; seqNo:416; 4061569.73 north; 594357.45 east; 36.70 lat; -121.94 lon; 27.90 depth; utmZone: 10 2024-11-06T04:38:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867881.17 2024-11-06T04:38:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:05Z- waiting for 1730867885.17, time is 1730867885.22 2024-11-06T04:38:05Z- LcmTrn::updateTrn() >>>> pose time 1730867885.22, meas time 1730867884.45 <<<< 2024-11-06T04:38:05Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867885.22 2024-11-06T04:38:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867884.45 sec, ping # 0; waiting for INS data more recent than 1730867881.17... 2024-11-06T04:38:05Z- TerrainNav::motionUpdate() - currEstimate = (4061570, 594357.45, 27.90) 2024-11-06T04:38:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:38:05Z- TerrainNav:: SubcloudNIS is 0.28, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:38:05Z- TNavPF::Projecting Measurements 2024-11-06T04:38:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:38:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:38:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:38:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:38:05Z- TNavFilter::4 good beams. 2024-11-06T04:38:05Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:38:05Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:38:05Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:38:05Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:38:05Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:38:05Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:38:05Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:38:05Z- TerrainMapOctree: (vehN, vehE ) = (4061569.37, 594357.92). 2024-11-06T04:38:05Z- TerrainMapOctree: Min Distance = 210.41. 2024-11-06T04:38:05Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:38:05Z- Forcing Subcloud Comparison 2024-11-06T04:38:05Z- Weighting particles with subcloud comparison 2024-11-06T04:38:05Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:05Z- meanExpectedMeasDiff: -0.276045 delta_rms_squared: 1.931971 alpha: 0.989754 2024-11-06T04:38:05Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:05Z- meanExpectedMeasDiff: 1.046555 delta_rms_squared: 1.432167 alpha: 0.986227 2024-11-06T04:38:05Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:05Z- meanExpectedMeasDiff: 0.262486 delta_rms_squared: 1.281893 alpha: 0.984638 2024-11-06T04:38:05Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:05Z- meanExpectedMeasDiff: 0.864999 delta_rms_squared: 2.214380 alpha: 0.991049 2024-11-06T04:38:05Z- Subcloud NIS: 0.270623 2024-11-06T04:38:05Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:38:05Z- TNavPF:: sumWeights = 0.685745 2024-11-06T04:38:05Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:38:05Z- TNavPF::First Term of Map Covariance Matrix is 0.76 2024-11-06T04:38:05Z- TNavPF::Calculated NIS Value : 0.09 numBeams normalized NIS: 0.02 2024-11-06T04:38:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867884.45 sec. 2024-11-06T04:38:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:38:05Z- variances([0], [2], and psi_berg) = 0.038393 0.035668 0.000000 2024-11-06T04:38:05Z- publishEstimatesDecoder() - at 1730867885.22 sec 2024-11-06T04:38:05Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:38:05Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061513.58 594361.62 36.89 2024-11-06T04:38:05Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061513.70 594361.55 37.58 lat/lon: 36.69 -121.94 2024-11-06T04:38:05Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.04 0.04 0.90 0.00 2024-11-06T04:38:05Z- handleDepth()- Depth_Keller msg: 1730867885.61 epoch sec; seqNo:417; depth 27.99 2024-11-06T04:38:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867885.62 epoch sec; seqNo:417; 4061569.73 north; 594357.45 east; 36.70 lat; -121.94 lon; 27.99 depth; utmZone: 10 2024-11-06T04:38:05Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:38:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867885.22 2024-11-06T04:38:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:05Z- waiting for 1730867889.22, time is 1730867885.62 2024-11-06T04:38:06Z- handleDepth()- Depth_Keller msg: 1730867886.01 epoch sec; seqNo:418; depth 28.05 2024-11-06T04:38:06Z- handleDVL() - DVL msg epochMillisec: 1730867886.01 2024-11-06T04:38:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:38:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:38:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:38:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:38:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:38:06Z- handleDvl() - RDI_Pathfinder msg: 1730867886.01 sec; seqNo:137; #beams:0 2024-11-06T04:38:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:38:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.09 , -0.06 2024-11-06T04:38:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867886.03 epoch sec; seqNo:418; 4061570.11 north; 594357.45 east; 36.70 lat; -121.94 lon; 28.05 depth; utmZone: 10 2024-11-06T04:38:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867885.22 2024-11-06T04:38:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:06Z- waiting for 1730867889.22, time is 1730867886.03 2024-11-06T04:38:06Z- AHRS_M2 msg: 1730867886.41 epoch sec; seqNo:383 2024-11-06T04:38:06Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.40 psi 2024-11-06T04:38:06Z- handleDepth()- Depth_Keller msg: 1730867886.42 epoch sec; seqNo:419; depth 28.14 2024-11-06T04:38:06Z- handleDVL() - DVL msg epochMillisec: 1730867886.45 2024-11-06T04:38:06Z- handleDvl() - RDI_Pathfinder msg: 1730867886.45 sec; seqNo:138; #beams:4 2024-11-06T04:38:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.51 , 59.47 , 57.94 , 73.57 2024-11-06T04:38:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.09 , -0.06 2024-11-06T04:38:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867886.47 epoch sec; seqNo:419; 4061570.48 north; 594356.22 east; 36.70 lat; -121.94 lon; 28.14 depth; utmZone: 10 2024-11-06T04:38:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867885.22 2024-11-06T04:38:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:06Z- waiting for 1730867889.22, time is 1730867886.47 2024-11-06T04:38:06Z- AHRS_M2 msg: 1730867886.79 epoch sec; seqNo:384 2024-11-06T04:38:06Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.40 psi 2024-11-06T04:38:06Z- handleDepth()- Depth_Keller msg: 1730867886.79 epoch sec; seqNo:420; depth 28.23 2024-11-06T04:38:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867886.82 epoch sec; seqNo:420; 4061570.48 north; 594356.22 east; 36.70 lat; -121.94 lon; 28.23 depth; utmZone: 10 2024-11-06T04:38:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867885.22 2024-11-06T04:38:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:06Z- waiting for 1730867889.22, time is 1730867886.82 2024-11-06T04:38:07Z- AHRS_M2 msg: 1730867887.19 epoch sec; seqNo:385 2024-11-06T04:38:07Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.40 psi 2024-11-06T04:38:07Z- handleDepth()- Depth_Keller msg: 1730867887.20 epoch sec; seqNo:421; depth 28.29 2024-11-06T04:38:07Z- handleDVL() - DVL msg epochMillisec: 1730867887.20 2024-11-06T04:38:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:38:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:38:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:38:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:38:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:38:07Z- handleDvl() - RDI_Pathfinder msg: 1730867887.20 sec; seqNo:139; #beams:0 2024-11-06T04:38:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:38:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.09 , -0.06 2024-11-06T04:38:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867887.21 epoch sec; seqNo:421; 4061570.85 north; 594356.22 east; 36.70 lat; -121.94 lon; 28.29 depth; utmZone: 10 2024-11-06T04:38:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867885.22 2024-11-06T04:38:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:07Z- waiting for 1730867889.22, time is 1730867887.21 2024-11-06T04:38:07Z- AHRS_M2 msg: 1730867887.60 epoch sec; seqNo:386 2024-11-06T04:38:07Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.40 psi 2024-11-06T04:38:07Z- handleDepth()- Depth_Keller msg: 1730867887.60 epoch sec; seqNo:422; depth 28.36 2024-11-06T04:38:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867887.61 epoch sec; seqNo:422; 4061570.85 north; 594356.22 east; 36.70 lat; -121.94 lon; 28.36 depth; utmZone: 10 2024-11-06T04:38:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867885.22 2024-11-06T04:38:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:07Z- waiting for 1730867889.22, time is 1730867887.61 2024-11-06T04:38:08Z- handleDepth()- Depth_Keller msg: 1730867888.03 epoch sec; seqNo:423; depth 28.43 2024-11-06T04:38:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867888.04 epoch sec; seqNo:423; 4061571.22 north; 594354.99 east; 36.70 lat; -121.94 lon; 28.43 depth; utmZone: 10 2024-11-06T04:38:08Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:38:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867885.22 2024-11-06T04:38:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:08Z- waiting for 1730867889.22, time is 1730867888.04 2024-11-06T04:38:08Z- AHRS_M2 msg: 1730867888.42 epoch sec; seqNo:387 2024-11-06T04:38:08Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.39 psi 2024-11-06T04:38:08Z- handleDepth()- Depth_Keller msg: 1730867888.43 epoch sec; seqNo:424; depth 28.50 2024-11-06T04:38:08Z- handleDVL() - DVL msg epochMillisec: 1730867888.45 2024-11-06T04:38:08Z- handleDvl() - RDI_Pathfinder msg: 1730867888.45 sec; seqNo:140; #beams:4 2024-11-06T04:38:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.51 , 58.86 , 57.94 , 73.57 2024-11-06T04:38:08Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.09 , -0.05 2024-11-06T04:38:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867888.46 epoch sec; seqNo:424; 4061571.22 north; 594354.99 east; 36.70 lat; -121.94 lon; 28.50 depth; utmZone: 10 2024-11-06T04:38:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867885.22 2024-11-06T04:38:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:08Z- waiting for 1730867889.22, time is 1730867888.46 2024-11-06T04:38:08Z- AHRS_M2 msg: 1730867888.82 epoch sec; seqNo:388 2024-11-06T04:38:08Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.39 psi 2024-11-06T04:38:08Z- handleDepth()- Depth_Keller msg: 1730867888.83 epoch sec; seqNo:425; depth 28.60 2024-11-06T04:38:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867888.85 epoch sec; seqNo:425; 4061571.60 north; 594354.99 east; 36.70 lat; -121.94 lon; 28.60 depth; utmZone: 10 2024-11-06T04:38:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867885.22 2024-11-06T04:38:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:08Z- waiting for 1730867889.22, time is 1730867888.85 2024-11-06T04:38:09Z- AHRS_M2 msg: 1730867889.22 epoch sec; seqNo:389 2024-11-06T04:38:09Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.38 psi 2024-11-06T04:38:09Z- handleDepth()- Depth_Keller msg: 1730867889.23 epoch sec; seqNo:426; depth 28.69 2024-11-06T04:38:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867889.24 epoch sec; seqNo:426; 4061571.98 north; 594354.98 east; 36.70 lat; -121.94 lon; 28.69 depth; utmZone: 10 2024-11-06T04:38:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867885.22 2024-11-06T04:38:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:09Z- waiting for 1730867889.22, time is 1730867889.24 2024-11-06T04:38:09Z- LcmTrn::updateTrn() >>>> pose time 1730867889.24, meas time 1730867888.45 <<<< 2024-11-06T04:38:09Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867889.24 2024-11-06T04:38:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867888.45 sec, ping # 0; waiting for INS data more recent than 1730867885.22... 2024-11-06T04:38:09Z- TerrainNav::motionUpdate() - currEstimate = (4061572, 594354.98, 28.69) 2024-11-06T04:38:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:38:09Z- TerrainNav:: SubcloudNIS is 0.27, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:38:09Z- TNavPF::Projecting Measurements 2024-11-06T04:38:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:38:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:38:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:38:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:38:09Z- TNavFilter::4 good beams. 2024-11-06T04:38:09Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:38:09Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:38:09Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:38:09Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:38:09Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:38:09Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:38:09Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:38:09Z- TerrainMapOctree: (vehN, vehE ) = (4061571.54, 594355.47). 2024-11-06T04:38:09Z- TerrainMapOctree: Min Distance = 207.30. 2024-11-06T04:38:09Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:38:09Z- Forcing Subcloud Comparison 2024-11-06T04:38:09Z- Weighting particles with subcloud comparison 2024-11-06T04:38:09Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:09Z- meanExpectedMeasDiff: -0.148055 delta_rms_squared: 1.929870 alpha: 0.989743 2024-11-06T04:38:09Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:09Z- meanExpectedMeasDiff: 0.602988 delta_rms_squared: 1.408872 alpha: 0.986003 2024-11-06T04:38:09Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:09Z- meanExpectedMeasDiff: 0.562282 delta_rms_squared: 1.269220 alpha: 0.984487 2024-11-06T04:38:09Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:09Z- meanExpectedMeasDiff: 0.927407 delta_rms_squared: 2.200009 alpha: 0.990991 2024-11-06T04:38:09Z- Subcloud NIS: 0.251407 2024-11-06T04:38:09Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:38:09Z- TNavPF:: sumWeights = 0.702985 2024-11-06T04:38:09Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:38:09Z- TNavPF::First Term of Map Covariance Matrix is 0.78 2024-11-06T04:38:09Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.01 2024-11-06T04:38:09Z- TerrainNav::Resampling particle filter... 2024-11-06T04:38:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867888.45 sec. 2024-11-06T04:38:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:38:09Z- variances([0], [2], and psi_berg) = 0.040462 0.037303 0.000000 2024-11-06T04:38:09Z- publishEstimatesDecoder() - at 1730867889.24 sec 2024-11-06T04:38:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:38:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061515.74 594359.05 36.30 2024-11-06T04:38:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061515.95 594359.08 38.19 lat/lon: 36.69 -121.94 2024-11-06T04:38:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.04 0.04 0.90 0.00 2024-11-06T04:38:09Z- AHRS_M2 msg: 1730867889.62 epoch sec; seqNo:390 2024-11-06T04:38:09Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.38 psi 2024-11-06T04:38:09Z- handleDepth()- Depth_Keller msg: 1730867889.63 epoch sec; seqNo:427; depth 28.75 2024-11-06T04:38:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867889.63 epoch sec; seqNo:427; 4061571.96 north; 594353.76 east; 36.70 lat; -121.94 lon; 28.75 depth; utmZone: 10 2024-11-06T04:38:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867889.24 2024-11-06T04:38:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:09Z- waiting for 1730867893.24, time is 1730867889.63 2024-11-06T04:38:10Z- AHRS_M2 msg: 1730867890.03 epoch sec; seqNo:391 2024-11-06T04:38:10Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.38 psi 2024-11-06T04:38:10Z- handleDepth()- Depth_Keller msg: 1730867890.03 epoch sec; seqNo:428; depth 28.82 2024-11-06T04:38:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867890.04 epoch sec; seqNo:428; 4061572.34 north; 594353.76 east; 36.70 lat; -121.94 lon; 28.82 depth; utmZone: 10 2024-11-06T04:38:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867889.24 2024-11-06T04:38:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:10Z- waiting for 1730867893.24, time is 1730867890.04 2024-11-06T04:38:10Z- AHRS_M2 msg: 1730867890.42 epoch sec; seqNo:392 2024-11-06T04:38:10Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.37 psi 2024-11-06T04:38:10Z- handleDepth()- Depth_Keller msg: 1730867890.43 epoch sec; seqNo:429; depth 28.91 2024-11-06T04:38:10Z- handleDVL() - DVL msg epochMillisec: 1730867890.45 2024-11-06T04:38:10Z- handleDvl() - RDI_Pathfinder msg: 1730867890.45 sec; seqNo:141; #beams:4 2024-11-06T04:38:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 70.20 , 58.55 , 57.33 , 72.35 2024-11-06T04:38:10Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , -0.09 , 0.05 2024-11-06T04:38:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867890.46 epoch sec; seqNo:429; 4061572.34 north; 594353.76 east; 36.70 lat; -121.94 lon; 28.91 depth; utmZone: 10 2024-11-06T04:38:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867889.24 2024-11-06T04:38:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:10Z- waiting for 1730867893.24, time is 1730867890.46 2024-11-06T04:38:10Z- AHRS_M2 msg: 1730867890.83 epoch sec; seqNo:393 2024-11-06T04:38:10Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.37 psi 2024-11-06T04:38:10Z- handleDepth()- Depth_Keller msg: 1730867890.85 epoch sec; seqNo:430; depth 28.99 2024-11-06T04:38:10Z- handleDVL() - DVL msg epochMillisec: 1730867890.85 2024-11-06T04:38:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:38:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:38:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:38:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:38:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:38:10Z- handleDvl() - RDI_Pathfinder msg: 1730867890.85 sec; seqNo:142; #beams:0 2024-11-06T04:38:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:38:10Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , -0.09 , 0.05 2024-11-06T04:38:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867890.87 epoch sec; seqNo:430; 4061572.71 north; 594352.53 east; 36.70 lat; -121.94 lon; 28.99 depth; utmZone: 10 2024-11-06T04:38:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867889.24 2024-11-06T04:38:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:10Z- waiting for 1730867893.24, time is 1730867890.87 2024-11-06T04:38:11Z- AHRS_M2 msg: 1730867891.23 epoch sec; seqNo:394 2024-11-06T04:38:11Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.36 psi 2024-11-06T04:38:11Z- handleDepth()- Depth_Keller msg: 1730867891.24 epoch sec; seqNo:431; depth 29.08 2024-11-06T04:38:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867891.25 epoch sec; seqNo:431; 4061573.09 north; 594352.53 east; 36.70 lat; -121.94 lon; 29.08 depth; utmZone: 10 2024-11-06T04:38:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867889.24 2024-11-06T04:38:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:11Z- waiting for 1730867893.24, time is 1730867891.25 2024-11-06T04:38:11Z- AHRS_M2 msg: 1730867891.64 epoch sec; seqNo:395 2024-11-06T04:38:11Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.36 psi 2024-11-06T04:38:11Z- handleDepth()- Depth_Keller msg: 1730867891.65 epoch sec; seqNo:432; depth 29.16 2024-11-06T04:38:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867891.66 epoch sec; seqNo:432; 4061573.09 north; 594352.53 east; 36.70 lat; -121.94 lon; 29.16 depth; utmZone: 10 2024-11-06T04:38:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867889.24 2024-11-06T04:38:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:11Z- waiting for 1730867893.24, time is 1730867891.66 2024-11-06T04:38:12Z- AHRS_M2 msg: 1730867892.04 epoch sec; seqNo:396 2024-11-06T04:38:12Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.36 psi 2024-11-06T04:38:12Z- handleDepth()- Depth_Keller msg: 1730867892.06 epoch sec; seqNo:433; depth 29.25 2024-11-06T04:38:12Z- handleDVL() - DVL msg epochMillisec: 1730867892.06 2024-11-06T04:38:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:38:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:38:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:38:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:38:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:38:12Z- handleDvl() - RDI_Pathfinder msg: 1730867892.06 sec; seqNo:143; #beams:0 2024-11-06T04:38:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:38:12Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , -0.09 , 0.05 2024-11-06T04:38:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867892.07 epoch sec; seqNo:433; 4061573.47 north; 594352.53 east; 36.70 lat; -121.94 lon; 29.25 depth; utmZone: 10 2024-11-06T04:38:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867889.24 2024-11-06T04:38:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:12Z- waiting for 1730867893.24, time is 1730867892.07 2024-11-06T04:38:12Z- handleDepth()- Depth_Keller msg: 1730867892.47 epoch sec; seqNo:434; depth 29.32 2024-11-06T04:38:12Z- handleDVL() - DVL msg epochMillisec: 1730867892.49 2024-11-06T04:38:12Z- handleDvl() - RDI_Pathfinder msg: 1730867892.49 sec; seqNo:144; #beams:4 2024-11-06T04:38:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.59 , 58.55 , 57.33 , 71.74 2024-11-06T04:38:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.10 , 0.10 2024-11-06T04:38:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867892.51 epoch sec; seqNo:434; 4061573.45 north; 594351.31 east; 36.70 lat; -121.94 lon; 29.32 depth; utmZone: 10 2024-11-06T04:38:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867889.24 2024-11-06T04:38:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:12Z- waiting for 1730867893.24, time is 1730867892.51 2024-11-06T04:38:12Z- AHRS_M2 msg: 1730867892.85 epoch sec; seqNo:397 2024-11-06T04:38:12Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.36 psi 2024-11-06T04:38:12Z- handleDepth()- Depth_Keller msg: 1730867892.86 epoch sec; seqNo:435; depth 29.41 2024-11-06T04:38:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867892.87 epoch sec; seqNo:435; 4061573.83 north; 594351.30 east; 36.70 lat; -121.94 lon; 29.41 depth; utmZone: 10 2024-11-06T04:38:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867889.24 2024-11-06T04:38:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:13Z- waiting for 1730867893.24, time is 1730867892.87 2024-11-06T04:38:13Z- AHRS_M2 msg: 1730867893.25 epoch sec; seqNo:398 2024-11-06T04:38:13Z- AHRS_M2 msg: 0.14 phi; -0.24 theta; 5.37 psi 2024-11-06T04:38:13Z- handleDepth()- Depth_Keller msg: 1730867893.26 epoch sec; seqNo:436; depth 29.48 2024-11-06T04:38:13Z- handleDVL() - DVL msg epochMillisec: 1730867893.26 2024-11-06T04:38:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:38:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:38:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:38:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:38:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:38:13Z- handleDvl() - RDI_Pathfinder msg: 1730867893.26 sec; seqNo:145; #beams:0 2024-11-06T04:38:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:38:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.10 , 0.10 2024-11-06T04:38:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867893.27 epoch sec; seqNo:436; 4061573.83 north; 594351.30 east; 36.70 lat; -121.94 lon; 29.48 depth; utmZone: 10 2024-11-06T04:38:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867889.24 2024-11-06T04:38:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:13Z- waiting for 1730867893.24, time is 1730867893.27 2024-11-06T04:38:13Z- LcmTrn::updateTrn() >>>> pose time 1730867893.27, meas time 1730867893.26 <<<< 2024-11-06T04:38:13Z- LcmTrn::updateTrn() equating pose 1730867893.26 and meas times 1730867893.26 2024-11-06T04:38:13Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867893.26 2024-11-06T04:38:13Z- TerrainNav::motionUpdate() - currEstimate = (4061574, 594351.30, 29.48) 2024-11-06T04:38:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:38:13Z- TerrainNav:: SubcloudNIS is 0.25, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:38:13Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:38:13Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:38:13Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:38:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:38:13Z- variances([0], [2], and psi_berg) = 0.043050 0.040128 0.000000 2024-11-06T04:38:13Z- publishEstimatesDecoder() - at 1730867893.26 sec 2024-11-06T04:38:13Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:38:13Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061517.61 594355.23 36.76 2024-11-06T04:38:13Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061517.80 594355.40 38.98 lat/lon: 36.69 -121.94 2024-11-06T04:38:13Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.04 0.04 1.16 0.00 2024-11-06T04:38:13Z- AHRS_M2 msg: 1730867893.65 epoch sec; seqNo:399 2024-11-06T04:38:13Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.37 psi 2024-11-06T04:38:13Z- handleDepth()- Depth_Keller msg: 1730867893.67 epoch sec; seqNo:437; depth 29.57 2024-11-06T04:38:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867893.69 epoch sec; seqNo:437; 4061574.20 north; 594350.08 east; 36.70 lat; -121.94 lon; 29.57 depth; utmZone: 10 2024-11-06T04:38:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867893.26 2024-11-06T04:38:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:13Z- waiting for 1730867897.26, time is 1730867893.69 2024-11-06T04:38:14Z- AHRS_M2 msg: 1730867894.06 epoch sec; seqNo:400 2024-11-06T04:38:14Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.38 psi 2024-11-06T04:38:14Z- handleDepth()- Depth_Keller msg: 1730867894.07 epoch sec; seqNo:438; depth 29.65 2024-11-06T04:38:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867894.08 epoch sec; seqNo:438; 4061574.20 north; 594350.08 east; 36.70 lat; -121.94 lon; 29.65 depth; utmZone: 10 2024-11-06T04:38:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867893.26 2024-11-06T04:38:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:14Z- waiting for 1730867897.26, time is 1730867894.08 2024-11-06T04:38:14Z- AHRS_M2 msg: 1730867894.47 epoch sec; seqNo:401 2024-11-06T04:38:14Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.38 psi 2024-11-06T04:38:14Z- handleDepth()- Depth_Keller msg: 1730867894.47 epoch sec; seqNo:439; depth 29.73 2024-11-06T04:38:14Z- handleDVL() - DVL msg epochMillisec: 1730867894.54 2024-11-06T04:38:14Z- handleDvl() - RDI_Pathfinder msg: 1730867894.54 sec; seqNo:146; #beams:4 2024-11-06T04:38:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 57.94 , 56.41 , 72.35 2024-11-06T04:38:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.10 , 0.01 2024-11-06T04:38:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867894.60 epoch sec; seqNo:439; 4061574.58 north; 594350.07 east; 36.70 lat; -121.94 lon; 29.73 depth; utmZone: 10 2024-11-06T04:38:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867893.26 2024-11-06T04:38:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:14Z- waiting for 1730867897.26, time is 1730867894.60 2024-11-06T04:38:14Z- AHRS_M2 msg: 1730867894.87 epoch sec; seqNo:402 2024-11-06T04:38:14Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.38 psi 2024-11-06T04:38:14Z- handleDepth()- Depth_Keller msg: 1730867894.88 epoch sec; seqNo:440; depth 29.83 2024-11-06T04:38:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867894.89 epoch sec; seqNo:440; 4061574.58 north; 594350.07 east; 36.70 lat; -121.94 lon; 29.83 depth; utmZone: 10 2024-11-06T04:38:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867893.26 2024-11-06T04:38:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:15Z- waiting for 1730867897.26, time is 1730867894.89 2024-11-06T04:38:15Z- AHRS_M2 msg: 1730867895.27 epoch sec; seqNo:403 2024-11-06T04:38:15Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.38 psi 2024-11-06T04:38:15Z- handleDepth()- Depth_Keller msg: 1730867895.28 epoch sec; seqNo:441; depth 29.89 2024-11-06T04:38:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867895.30 epoch sec; seqNo:441; 4061574.94 north; 594348.85 east; 36.70 lat; -121.94 lon; 29.89 depth; utmZone: 10 2024-11-06T04:38:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867893.26 2024-11-06T04:38:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:15Z- waiting for 1730867897.26, time is 1730867895.30 2024-11-06T04:38:15Z- AHRS_M2 msg: 1730867895.68 epoch sec; seqNo:404 2024-11-06T04:38:15Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.38 psi 2024-11-06T04:38:15Z- handleDepth()- Depth_Keller msg: 1730867895.69 epoch sec; seqNo:442; depth 29.97 2024-11-06T04:38:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867895.70 epoch sec; seqNo:442; 4061574.94 north; 594348.85 east; 36.70 lat; -121.94 lon; 29.97 depth; utmZone: 10 2024-11-06T04:38:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867893.26 2024-11-06T04:38:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:15Z- waiting for 1730867897.26, time is 1730867895.70 2024-11-06T04:38:16Z- AHRS_M2 msg: 1730867896.08 epoch sec; seqNo:405 2024-11-06T04:38:16Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.39 psi 2024-11-06T04:38:16Z- handleDepth()- Depth_Keller msg: 1730867896.09 epoch sec; seqNo:443; depth 30.05 2024-11-06T04:38:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867896.10 epoch sec; seqNo:443; 4061575.32 north; 594348.84 east; 36.70 lat; -121.94 lon; 30.05 depth; utmZone: 10 2024-11-06T04:38:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867893.26 2024-11-06T04:38:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:16Z- waiting for 1730867897.26, time is 1730867896.10 2024-11-06T04:38:16Z- AHRS_M2 msg: 1730867896.49 epoch sec; seqNo:406 2024-11-06T04:38:16Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.39 psi 2024-11-06T04:38:16Z- handleDepth()- Depth_Keller msg: 1730867896.49 epoch sec; seqNo:444; depth 30.12 2024-11-06T04:38:16Z- handleDVL() - DVL msg epochMillisec: 1730867896.51 2024-11-06T04:38:16Z- handleDvl() - RDI_Pathfinder msg: 1730867896.51 sec; seqNo:147; #beams:4 2024-11-06T04:38:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 57.93 , 56.40 , 72.03 2024-11-06T04:38:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.11 , -0.01 2024-11-06T04:38:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867896.52 epoch sec; seqNo:444; 4061575.32 north; 594348.84 east; 36.70 lat; -121.94 lon; 30.12 depth; utmZone: 10 2024-11-06T04:38:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867893.26 2024-11-06T04:38:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:16Z- waiting for 1730867897.26, time is 1730867896.52 2024-11-06T04:38:16Z- AHRS_M2 msg: 1730867896.89 epoch sec; seqNo:407 2024-11-06T04:38:16Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.39 psi 2024-11-06T04:38:16Z- handleDepth()- Depth_Keller msg: 1730867896.89 epoch sec; seqNo:445; depth 30.21 2024-11-06T04:38:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867896.91 epoch sec; seqNo:445; 4061575.69 north; 594347.62 east; 36.70 lat; -121.94 lon; 30.21 depth; utmZone: 10 2024-11-06T04:38:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867893.26 2024-11-06T04:38:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:17Z- waiting for 1730867897.26, time is 1730867896.91 2024-11-06T04:38:17Z- AHRS_M2 msg: 1730867897.30 epoch sec; seqNo:408 2024-11-06T04:38:17Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.39 psi 2024-11-06T04:38:17Z- handleDepth()- Depth_Keller msg: 1730867897.31 epoch sec; seqNo:446; depth 30.29 2024-11-06T04:38:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867897.32 epoch sec; seqNo:446; 4061575.69 north; 594347.62 east; 36.70 lat; -121.94 lon; 30.29 depth; utmZone: 10 2024-11-06T04:38:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867893.26 2024-11-06T04:38:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:17Z- waiting for 1730867897.26, time is 1730867897.32 2024-11-06T04:38:17Z- LcmTrn::updateTrn() >>>> pose time 1730867897.32, meas time 1730867896.51 <<<< 2024-11-06T04:38:17Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867897.32 2024-11-06T04:38:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867896.51 sec, ping # 0; waiting for INS data more recent than 1730867893.26... 2024-11-06T04:38:17Z- TerrainNav::motionUpdate() - currEstimate = (4061576, 594347.62, 30.29) 2024-11-06T04:38:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:38:17Z- TerrainNav:: SubcloudNIS is 0.25, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:38:17Z- TNavPF::Projecting Measurements 2024-11-06T04:38:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:38:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:38:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:38:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:38:17Z- TNavFilter::4 good beams. 2024-11-06T04:38:17Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:38:17Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:38:17Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:38:17Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:38:17Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:38:17Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:38:17Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:38:17Z- TerrainMapOctree: (vehN, vehE ) = (4061575.32, 594348.35). 2024-11-06T04:38:17Z- TerrainMapOctree: Min Distance = 200.47. 2024-11-06T04:38:17Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:38:17Z- Forcing Subcloud Comparison 2024-11-06T04:38:17Z- Weighting particles with subcloud comparison 2024-11-06T04:38:17Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:17Z- meanExpectedMeasDiff: 0.022467 delta_rms_squared: 2.449353 alpha: 0.991901 2024-11-06T04:38:17Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:17Z- meanExpectedMeasDiff: 0.322558 delta_rms_squared: 1.727171 alpha: 0.988553 2024-11-06T04:38:17Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:17Z- meanExpectedMeasDiff: 0.646283 delta_rms_squared: 1.556240 alpha: 0.987312 2024-11-06T04:38:17Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:17Z- meanExpectedMeasDiff: 0.263354 delta_rms_squared: 2.792653 alpha: 0.992889 2024-11-06T04:38:17Z- Subcloud NIS: 0.240794 2024-11-06T04:38:17Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:38:17Z- TNavPF:: sumWeights = 0.686949 2024-11-06T04:38:17Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:38:17Z- TNavPF::First Term of Map Covariance Matrix is 0.84 2024-11-06T04:38:17Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T04:38:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867896.51 sec. 2024-11-06T04:38:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:38:17Z- variances([0], [2], and psi_berg) = 0.046657 0.042659 0.000000 2024-11-06T04:38:17Z- publishEstimatesDecoder() - at 1730867897.32 sec 2024-11-06T04:38:17Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:38:17Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061519.38 594351.65 39.21 2024-11-06T04:38:17Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061519.66 594351.72 39.67 lat/lon: 36.69 -121.94 2024-11-06T04:38:17Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.05 0.04 0.97 0.00 2024-11-06T04:38:17Z- AHRS_M2 msg: 1730867897.70 epoch sec; seqNo:409 2024-11-06T04:38:17Z- AHRS_M2 msg: 0.15 phi; -0.25 theta; 5.39 psi 2024-11-06T04:38:17Z- handleDepth()- Depth_Keller msg: 1730867897.70 epoch sec; seqNo:447; depth 30.37 2024-11-06T04:38:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867897.71 epoch sec; seqNo:447; 4061576.06 north; 594347.62 east; 36.70 lat; -121.94 lon; 30.37 depth; utmZone: 10 2024-11-06T04:38:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867897.32 2024-11-06T04:38:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:17Z- waiting for 1730867901.32, time is 1730867897.71 2024-11-06T04:38:18Z- AHRS_M2 msg: 1730867898.10 epoch sec; seqNo:410 2024-11-06T04:38:18Z- AHRS_M2 msg: 0.15 phi; -0.25 theta; 5.40 psi 2024-11-06T04:38:18Z- handleDepth()- Depth_Keller msg: 1730867898.11 epoch sec; seqNo:448; depth 30.45 2024-11-06T04:38:18Z- handleDVL() - DVL msg epochMillisec: 1730867898.11 2024-11-06T04:38:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:38:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:38:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:38:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:38:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:38:18Z- handleDvl() - RDI_Pathfinder msg: 1730867898.11 sec; seqNo:148; #beams:0 2024-11-06T04:38:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:38:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.11 , -0.01 2024-11-06T04:38:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867898.12 epoch sec; seqNo:448; 4061576.06 north; 594347.62 east; 36.70 lat; -121.94 lon; 30.45 depth; utmZone: 10 2024-11-06T04:38:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867897.32 2024-11-06T04:38:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:18Z- waiting for 1730867901.32, time is 1730867898.12 2024-11-06T04:38:18Z- AHRS_M2 msg: 1730867898.50 epoch sec; seqNo:411 2024-11-06T04:38:18Z- AHRS_M2 msg: 0.15 phi; -0.25 theta; 5.40 psi 2024-11-06T04:38:18Z- handleDepth()- Depth_Keller msg: 1730867898.51 epoch sec; seqNo:449; depth 30.53 2024-11-06T04:38:18Z- handleDVL() - DVL msg epochMillisec: 1730867898.54 2024-11-06T04:38:18Z- handleDvl() - RDI_Pathfinder msg: 1730867898.54 sec; seqNo:149; #beams:4 2024-11-06T04:38:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 69.89 , 57.32 , 56.10 , 72.03 2024-11-06T04:38:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.10 , -0.00 2024-11-06T04:38:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867898.55 epoch sec; seqNo:449; 4061576.43 north; 594346.39 east; 36.70 lat; -121.94 lon; 30.53 depth; utmZone: 10 2024-11-06T04:38:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867897.32 2024-11-06T04:38:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:18Z- waiting for 1730867901.32, time is 1730867898.55 2024-11-06T04:38:18Z- AHRS_M2 msg: 1730867898.91 epoch sec; seqNo:412 2024-11-06T04:38:18Z- AHRS_M2 msg: 0.15 phi; -0.25 theta; 5.41 psi 2024-11-06T04:38:18Z- handleDepth()- Depth_Keller msg: 1730867898.92 epoch sec; seqNo:450; depth 30.62 2024-11-06T04:38:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867898.95 epoch sec; seqNo:450; 4061576.43 north; 594346.39 east; 36.70 lat; -121.94 lon; 30.62 depth; utmZone: 10 2024-11-06T04:38:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867897.32 2024-11-06T04:38:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:19Z- waiting for 1730867901.32, time is 1730867898.95 2024-11-06T04:38:19Z- AHRS_M2 msg: 1730867899.31 epoch sec; seqNo:413 2024-11-06T04:38:19Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.41 psi 2024-11-06T04:38:19Z- handleDepth()- Depth_Keller msg: 1730867899.32 epoch sec; seqNo:451; depth 30.70 2024-11-06T04:38:19Z- handleDVL() - DVL msg epochMillisec: 1730867899.33 2024-11-06T04:38:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:38:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:38:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:38:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:38:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:38:19Z- handleDvl() - RDI_Pathfinder msg: 1730867899.33 sec; seqNo:150; #beams:0 2024-11-06T04:38:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:38:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.10 , -0.00 2024-11-06T04:38:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867899.34 epoch sec; seqNo:451; 4061576.81 north; 594346.39 east; 36.70 lat; -121.94 lon; 30.70 depth; utmZone: 10 2024-11-06T04:38:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867897.32 2024-11-06T04:38:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:19Z- waiting for 1730867901.32, time is 1730867899.34 2024-11-06T04:38:19Z- AHRS_M2 msg: 1730867899.72 epoch sec; seqNo:414 2024-11-06T04:38:19Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.41 psi 2024-11-06T04:38:19Z- handleDepth()- Depth_Keller msg: 1730867899.72 epoch sec; seqNo:452; depth 30.78 2024-11-06T04:38:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867899.73 epoch sec; seqNo:452; 4061576.81 north; 594346.39 east; 36.70 lat; -121.94 lon; 30.78 depth; utmZone: 10 2024-11-06T04:38:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867897.32 2024-11-06T04:38:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:19Z- waiting for 1730867901.32, time is 1730867899.73 2024-11-06T04:38:20Z- AHRS_M2 msg: 1730867900.12 epoch sec; seqNo:415 2024-11-06T04:38:20Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.42 psi 2024-11-06T04:38:20Z- handleDepth()- Depth_Keller msg: 1730867900.13 epoch sec; seqNo:453; depth 30.85 2024-11-06T04:38:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867900.14 epoch sec; seqNo:453; 4061577.17 north; 594345.16 east; 36.70 lat; -121.94 lon; 30.85 depth; utmZone: 10 2024-11-06T04:38:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867897.32 2024-11-06T04:38:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:20Z- waiting for 1730867901.32, time is 1730867900.14 2024-11-06T04:38:20Z- AHRS_M2 msg: 1730867900.53 epoch sec; seqNo:416 2024-11-06T04:38:20Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.42 psi 2024-11-06T04:38:20Z- handleDepth()- Depth_Keller msg: 1730867900.54 epoch sec; seqNo:454; depth 30.91 2024-11-06T04:38:20Z- handleDVL() - DVL msg epochMillisec: 1730867900.57 2024-11-06T04:38:20Z- handleDvl() - RDI_Pathfinder msg: 1730867900.57 sec; seqNo:151; #beams:4 2024-11-06T04:38:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.97 , 57.32 , 55.48 , 71.73 2024-11-06T04:38:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.10 , -0.05 2024-11-06T04:38:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867900.59 epoch sec; seqNo:454; 4061577.55 north; 594345.16 east; 36.70 lat; -121.94 lon; 30.91 depth; utmZone: 10 2024-11-06T04:38:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867897.32 2024-11-06T04:38:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:20Z- waiting for 1730867901.32, time is 1730867900.59 2024-11-06T04:38:20Z- AHRS_M2 msg: 1730867900.93 epoch sec; seqNo:417 2024-11-06T04:38:20Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.43 psi 2024-11-06T04:38:20Z- handleDepth()- Depth_Keller msg: 1730867900.93 epoch sec; seqNo:455; depth 31.02 2024-11-06T04:38:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867900.95 epoch sec; seqNo:455; 4061577.55 north; 594345.16 east; 36.70 lat; -121.94 lon; 31.02 depth; utmZone: 10 2024-11-06T04:38:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867897.32 2024-11-06T04:38:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:21Z- waiting for 1730867901.32, time is 1730867900.95 2024-11-06T04:38:21Z- AHRS_M2 msg: 1730867901.33 epoch sec; seqNo:418 2024-11-06T04:38:21Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.43 psi 2024-11-06T04:38:21Z- handleDepth()- Depth_Keller msg: 1730867901.34 epoch sec; seqNo:456; depth 31.10 2024-11-06T04:38:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867901.36 epoch sec; seqNo:456; 4061577.93 north; 594345.15 east; 36.70 lat; -121.94 lon; 31.10 depth; utmZone: 10 2024-11-06T04:38:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867897.32 2024-11-06T04:38:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:21Z- waiting for 1730867901.32, time is 1730867901.36 2024-11-06T04:38:21Z- LcmTrn::updateTrn() >>>> pose time 1730867901.36, meas time 1730867900.57 <<<< 2024-11-06T04:38:21Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867901.36 2024-11-06T04:38:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867900.57 sec, ping # 0; waiting for INS data more recent than 1730867897.32... 2024-11-06T04:38:21Z- TerrainNav::motionUpdate() - currEstimate = (4061578, 594345.15, 31.10) 2024-11-06T04:38:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:38:21Z- TerrainNav:: SubcloudNIS is 0.24, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:38:21Z- TNavPF::Projecting Measurements 2024-11-06T04:38:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:38:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:38:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:38:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:38:21Z- TNavFilter::4 good beams. 2024-11-06T04:38:21Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:38:21Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:38:21Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:38:21Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:38:21Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:38:21Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:38:21Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:38:21Z- TerrainMapOctree: (vehN, vehE ) = (4061577.50, 594345.63). 2024-11-06T04:38:21Z- TerrainMapOctree: Min Distance = 197.24. 2024-11-06T04:38:21Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:38:21Z- Forcing Subcloud Comparison 2024-11-06T04:38:21Z- Weighting particles with subcloud comparison 2024-11-06T04:38:21Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:21Z- meanExpectedMeasDiff: 0.092489 delta_rms_squared: 2.089772 alpha: 0.990520 2024-11-06T04:38:21Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:21Z- meanExpectedMeasDiff: 0.219252 delta_rms_squared: 1.489948 alpha: 0.986755 2024-11-06T04:38:21Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:21Z- meanExpectedMeasDiff: 0.631467 delta_rms_squared: 1.321655 alpha: 0.985093 2024-11-06T04:38:21Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:21Z- meanExpectedMeasDiff: 0.297936 delta_rms_squared: 2.408907 alpha: 0.991766 2024-11-06T04:38:21Z- Subcloud NIS: 0.224921 2024-11-06T04:38:21Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:38:21Z- TNavPF:: sumWeights = 0.711494 2024-11-06T04:38:21Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:38:21Z- TNavPF::First Term of Map Covariance Matrix is 0.79 2024-11-06T04:38:21Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T04:38:21Z- TerrainNav::Resampling particle filter... 2024-11-06T04:38:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867900.57 sec. 2024-11-06T04:38:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:38:21Z- variances([0], [2], and psi_berg) = 0.048854 0.044516 0.000000 2024-11-06T04:38:21Z- publishEstimatesDecoder() - at 1730867901.36 sec 2024-11-06T04:38:21Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:38:21Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061521.72 594349.10 40.04 2024-11-06T04:38:21Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061521.90 594349.26 40.35 lat/lon: 36.69 -121.94 2024-11-06T04:38:21Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.05 0.04 0.90 0.00 2024-11-06T04:38:21Z- AHRS_M2 msg: 1730867901.73 epoch sec; seqNo:419 2024-11-06T04:38:21Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.43 psi 2024-11-06T04:38:21Z- handleDepth()- Depth_Keller msg: 1730867901.75 epoch sec; seqNo:457; depth 31.19 2024-11-06T04:38:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867901.76 epoch sec; seqNo:457; 4061577.92 north; 594343.93 east; 36.70 lat; -121.94 lon; 31.19 depth; utmZone: 10 2024-11-06T04:38:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867901.36 2024-11-06T04:38:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:21Z- waiting for 1730867905.36, time is 1730867901.76 2024-11-06T04:38:22Z- AHRS_M2 msg: 1730867902.14 epoch sec; seqNo:420 2024-11-06T04:38:22Z- AHRS_M2 msg: 0.14 phi; -0.26 theta; 5.44 psi 2024-11-06T04:38:22Z- handleDepth()- Depth_Keller msg: 1730867902.14 epoch sec; seqNo:458; depth 31.27 2024-11-06T04:38:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867902.15 epoch sec; seqNo:458; 4061578.30 north; 594343.93 east; 36.70 lat; -121.94 lon; 31.27 depth; utmZone: 10 2024-11-06T04:38:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867901.36 2024-11-06T04:38:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:22Z- waiting for 1730867905.36, time is 1730867902.15 2024-11-06T04:38:22Z- AHRS_M2 msg: 1730867902.54 epoch sec; seqNo:421 2024-11-06T04:38:22Z- AHRS_M2 msg: 0.15 phi; -0.26 theta; 5.44 psi 2024-11-06T04:38:22Z- handleDepth()- Depth_Keller msg: 1730867902.55 epoch sec; seqNo:459; depth 31.36 2024-11-06T04:38:22Z- handleDVL() - DVL msg epochMillisec: 1730867902.58 2024-11-06T04:38:22Z- handleDvl() - RDI_Pathfinder msg: 1730867902.58 sec; seqNo:152; #beams:4 2024-11-06T04:38:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.32 , 57.03 , 55.00 , 72.37 2024-11-06T04:38:22Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.15 , -0.03 2024-11-06T04:38:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867902.60 epoch sec; seqNo:459; 4061578.68 north; 594343.93 east; 36.70 lat; -121.94 lon; 31.36 depth; utmZone: 10 2024-11-06T04:38:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867901.36 2024-11-06T04:38:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:22Z- waiting for 1730867905.36, time is 1730867902.60 2024-11-06T04:38:22Z- AHRS_M2 msg: 1730867902.95 epoch sec; seqNo:422 2024-11-06T04:38:22Z- AHRS_M2 msg: 0.15 phi; -0.26 theta; 5.44 psi 2024-11-06T04:38:22Z- handleDepth()- Depth_Keller msg: 1730867902.95 epoch sec; seqNo:460; depth 31.46 2024-11-06T04:38:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867902.97 epoch sec; seqNo:460; 4061578.68 north; 594343.93 east; 36.70 lat; -121.94 lon; 31.46 depth; utmZone: 10 2024-11-06T04:38:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867901.36 2024-11-06T04:38:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:23Z- waiting for 1730867905.36, time is 1730867902.97 2024-11-06T04:38:23Z- AHRS_M2 msg: 1730867903.36 epoch sec; seqNo:423 2024-11-06T04:38:23Z- AHRS_M2 msg: 0.15 phi; -0.26 theta; 5.44 psi 2024-11-06T04:38:23Z- handleDepth()- Depth_Keller msg: 1730867903.36 epoch sec; seqNo:461; depth 31.54 2024-11-06T04:38:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867903.37 epoch sec; seqNo:461; 4061579.04 north; 594342.70 east; 36.70 lat; -121.94 lon; 31.54 depth; utmZone: 10 2024-11-06T04:38:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867901.36 2024-11-06T04:38:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:23Z- waiting for 1730867905.36, time is 1730867903.37 2024-11-06T04:38:23Z- AHRS_M2 msg: 1730867903.76 epoch sec; seqNo:424 2024-11-06T04:38:23Z- AHRS_M2 msg: 0.15 phi; -0.26 theta; 5.45 psi 2024-11-06T04:38:23Z- handleDepth()- Depth_Keller msg: 1730867903.76 epoch sec; seqNo:462; depth 31.63 2024-11-06T04:38:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867903.77 epoch sec; seqNo:462; 4061579.04 north; 594342.70 east; 36.70 lat; -121.94 lon; 31.63 depth; utmZone: 10 2024-11-06T04:38:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867901.36 2024-11-06T04:38:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:23Z- waiting for 1730867905.36, time is 1730867903.77 2024-11-06T04:38:24Z- AHRS_M2 msg: 1730867904.16 epoch sec; seqNo:425 2024-11-06T04:38:24Z- AHRS_M2 msg: 0.15 phi; -0.26 theta; 5.45 psi 2024-11-06T04:38:24Z- handleDepth()- Depth_Keller msg: 1730867904.16 epoch sec; seqNo:463; depth 31.72 2024-11-06T04:38:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867904.17 epoch sec; seqNo:463; 4061579.42 north; 594342.70 east; 36.70 lat; -121.94 lon; 31.72 depth; utmZone: 10 2024-11-06T04:38:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867901.36 2024-11-06T04:38:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:24Z- waiting for 1730867905.36, time is 1730867904.17 2024-11-06T04:38:24Z- AHRS_M2 msg: 1730867904.57 epoch sec; seqNo:426 2024-11-06T04:38:24Z- AHRS_M2 msg: 0.15 phi; -0.26 theta; 5.46 psi 2024-11-06T04:38:24Z- handleDepth()- Depth_Keller msg: 1730867904.58 epoch sec; seqNo:464; depth 31.82 2024-11-06T04:38:24Z- handleDVL() - DVL msg epochMillisec: 1730867904.61 2024-11-06T04:38:24Z- handleDvl() - RDI_Pathfinder msg: 1730867904.61 sec; seqNo:153; #beams:4 2024-11-06T04:38:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.32 , 56.16 , 54.71 , 71.50 2024-11-06T04:38:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.18 , 0.06 2024-11-06T04:38:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867904.63 epoch sec; seqNo:464; 4061579.79 north; 594341.47 east; 36.70 lat; -121.94 lon; 31.82 depth; utmZone: 10 2024-11-06T04:38:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867901.36 2024-11-06T04:38:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:24Z- waiting for 1730867905.36, time is 1730867904.63 2024-11-06T04:38:24Z- AHRS_M2 msg: 1730867904.97 epoch sec; seqNo:427 2024-11-06T04:38:24Z- AHRS_M2 msg: 0.14 phi; -0.26 theta; 5.46 psi 2024-11-06T04:38:24Z- handleDepth()- Depth_Keller msg: 1730867904.97 epoch sec; seqNo:465; depth 31.92 2024-11-06T04:38:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867905.00 epoch sec; seqNo:465; 4061579.79 north; 594341.47 east; 36.70 lat; -121.94 lon; 31.92 depth; utmZone: 10 2024-11-06T04:38:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867901.36 2024-11-06T04:38:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:25Z- waiting for 1730867905.36, time is 1730867905.00 2024-11-06T04:38:25Z- AHRS_M2 msg: 1730867905.37 epoch sec; seqNo:428 2024-11-06T04:38:25Z- AHRS_M2 msg: 0.15 phi; -0.26 theta; 5.47 psi 2024-11-06T04:38:25Z- handleDepth()- Depth_Keller msg: 1730867905.38 epoch sec; seqNo:466; depth 32.00 2024-11-06T04:38:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867905.41 epoch sec; seqNo:466; 4061580.16 north; 594341.47 east; 36.70 lat; -121.94 lon; 32.00 depth; utmZone: 10 2024-11-06T04:38:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867901.36 2024-11-06T04:38:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:25Z- waiting for 1730867905.36, time is 1730867905.41 2024-11-06T04:38:25Z- LcmTrn::updateTrn() >>>> pose time 1730867905.41, meas time 1730867904.61 <<<< 2024-11-06T04:38:25Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867905.41 2024-11-06T04:38:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867904.61 sec, ping # 0; waiting for INS data more recent than 1730867901.36... 2024-11-06T04:38:25Z- TerrainNav::motionUpdate() - currEstimate = (4061580, 594341.47, 32.00) 2024-11-06T04:38:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:38:25Z- TerrainNav:: SubcloudNIS is 0.22, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:38:25Z- TNavPF::Projecting Measurements 2024-11-06T04:38:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:38:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:38:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:38:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:38:25Z- TNavFilter::4 good beams. 2024-11-06T04:38:25Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:38:25Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:38:25Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:38:25Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:38:25Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:38:25Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:38:25Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:38:25Z- TerrainMapOctree: (vehN, vehE ) = (4061579.72, 594342.20). 2024-11-06T04:38:25Z- TerrainMapOctree: Min Distance = 193.63. 2024-11-06T04:38:25Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:38:25Z- Forcing Subcloud Comparison 2024-11-06T04:38:25Z- Weighting particles with subcloud comparison 2024-11-06T04:38:25Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:25Z- meanExpectedMeasDiff: 0.319720 delta_rms_squared: 1.959752 alpha: 0.989898 2024-11-06T04:38:25Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:25Z- meanExpectedMeasDiff: -0.143080 delta_rms_squared: 1.394334 alpha: 0.985859 2024-11-06T04:38:25Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:25Z- meanExpectedMeasDiff: 0.724067 delta_rms_squared: 1.231696 alpha: 0.984022 2024-11-06T04:38:25Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:25Z- meanExpectedMeasDiff: -0.598538 delta_rms_squared: 2.292996 alpha: 0.991353 2024-11-06T04:38:25Z- Subcloud NIS: 0.215409 2024-11-06T04:38:25Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:38:25Z- TNavPF:: sumWeights = 0.695086 2024-11-06T04:38:25Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:38:25Z- TNavPF::First Term of Map Covariance Matrix is 0.74 2024-11-06T04:38:25Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T04:38:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867904.61 sec. 2024-11-06T04:38:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:38:25Z- variances([0], [2], and psi_berg) = 0.052474 0.047823 0.000000 2024-11-06T04:38:25Z- publishEstimatesDecoder() - at 1730867905.41 sec 2024-11-06T04:38:25Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:38:25Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061524.45 594345.51 42.29 2024-11-06T04:38:25Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061524.13 594345.57 41.23 lat/lon: 36.69 -121.94 2024-11-06T04:38:25Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.05 0.05 0.86 0.00 2024-11-06T04:38:25Z- AHRS_M2 msg: 1730867905.78 epoch sec; seqNo:429 2024-11-06T04:38:25Z- AHRS_M2 msg: 0.14 phi; -0.26 theta; 5.47 psi 2024-11-06T04:38:25Z- handleDepth()- Depth_Keller msg: 1730867905.79 epoch sec; seqNo:467; depth 32.08 2024-11-06T04:38:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867905.80 epoch sec; seqNo:467; 4061580.16 north; 594341.47 east; 36.70 lat; -121.94 lon; 32.08 depth; utmZone: 10 2024-11-06T04:38:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867905.41 2024-11-06T04:38:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:25Z- waiting for 1730867909.41, time is 1730867905.80 2024-11-06T04:38:26Z- AHRS_M2 msg: 1730867906.19 epoch sec; seqNo:430 2024-11-06T04:38:26Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.49 psi 2024-11-06T04:38:26Z- handleDepth()- Depth_Keller msg: 1730867906.19 epoch sec; seqNo:468; depth 32.20 2024-11-06T04:38:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867906.20 epoch sec; seqNo:468; 4061580.53 north; 594340.24 east; 36.70 lat; -121.94 lon; 32.20 depth; utmZone: 10 2024-11-06T04:38:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867905.41 2024-11-06T04:38:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:26Z- waiting for 1730867909.41, time is 1730867906.20 2024-11-06T04:38:26Z- AHRS_M2 msg: 1730867906.59 epoch sec; seqNo:431 2024-11-06T04:38:26Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.49 psi 2024-11-06T04:38:26Z- handleDepth()- Depth_Keller msg: 1730867906.60 epoch sec; seqNo:469; depth 32.28 2024-11-06T04:38:26Z- handleDVL() - DVL msg epochMillisec: 1730867906.62 2024-11-06T04:38:26Z- handleDvl() - RDI_Pathfinder msg: 1730867906.62 sec; seqNo:154; #beams:4 2024-11-06T04:38:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 68.03 , 56.16 , 54.71 , 70.92 2024-11-06T04:38:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.15 , 0.04 2024-11-06T04:38:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867906.64 epoch sec; seqNo:469; 4061580.53 north; 594340.24 east; 36.70 lat; -121.94 lon; 32.28 depth; utmZone: 10 2024-11-06T04:38:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867905.41 2024-11-06T04:38:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:26Z- waiting for 1730867909.41, time is 1730867906.64 2024-11-06T04:38:26Z- AHRS_M2 msg: 1730867906.99 epoch sec; seqNo:432 2024-11-06T04:38:26Z- AHRS_M2 msg: 0.14 phi; -0.25 theta; 5.50 psi 2024-11-06T04:38:26Z- handleDepth()- Depth_Keller msg: 1730867906.99 epoch sec; seqNo:470; depth 32.37 2024-11-06T04:38:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867907.01 epoch sec; seqNo:470; 4061580.91 north; 594340.24 east; 36.70 lat; -121.94 lon; 32.37 depth; utmZone: 10 2024-11-06T04:38:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867905.41 2024-11-06T04:38:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:27Z- waiting for 1730867909.41, time is 1730867907.01 2024-11-06T04:38:27Z- AHRS_M2 msg: 1730867907.39 epoch sec; seqNo:433 2024-11-06T04:38:27Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.51 psi 2024-11-06T04:38:27Z- handleDepth()- Depth_Keller msg: 1730867907.40 epoch sec; seqNo:471; depth 32.46 2024-11-06T04:38:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867907.41 epoch sec; seqNo:471; 4061580.91 north; 594340.24 east; 36.70 lat; -121.94 lon; 32.46 depth; utmZone: 10 2024-11-06T04:38:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867905.41 2024-11-06T04:38:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:27Z- waiting for 1730867909.41, time is 1730867907.41 2024-11-06T04:38:27Z- AHRS_M2 msg: 1730867907.80 epoch sec; seqNo:434 2024-11-06T04:38:27Z- AHRS_M2 msg: 0.15 phi; -0.24 theta; 5.51 psi 2024-11-06T04:38:27Z- handleDepth()- Depth_Keller msg: 1730867907.80 epoch sec; seqNo:472; depth 32.54 2024-11-06T04:38:27Z- handleDVL() - DVL msg epochMillisec: 1730867907.81 2024-11-06T04:38:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:38:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:38:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:38:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:38:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:38:27Z- handleDvl() - RDI_Pathfinder msg: 1730867907.81 sec; seqNo:155; #beams:0 2024-11-06T04:38:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:38:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.15 , 0.04 2024-11-06T04:38:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867907.83 epoch sec; seqNo:472; 4061581.27 north; 594339.01 east; 36.70 lat; -121.94 lon; 32.54 depth; utmZone: 10 2024-11-06T04:38:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867905.41 2024-11-06T04:38:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:27Z- waiting for 1730867909.41, time is 1730867907.83 2024-11-06T04:38:28Z- AHRS_M2 msg: 1730867908.20 epoch sec; seqNo:435 2024-11-06T04:38:28Z- AHRS_M2 msg: 0.14 phi; -0.24 theta; 5.51 psi 2024-11-06T04:38:28Z- handleDepth()- Depth_Keller msg: 1730867908.21 epoch sec; seqNo:473; depth 32.64 2024-11-06T04:38:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867908.22 epoch sec; seqNo:473; 4061581.27 north; 594339.01 east; 36.70 lat; -121.94 lon; 32.64 depth; utmZone: 10 2024-11-06T04:38:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867905.41 2024-11-06T04:38:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:28Z- waiting for 1730867909.41, time is 1730867908.22 2024-11-06T04:38:28Z- AHRS_M2 msg: 1730867908.61 epoch sec; seqNo:436 2024-11-06T04:38:28Z- AHRS_M2 msg: 0.14 phi; -0.24 theta; 5.52 psi 2024-11-06T04:38:28Z- handleDepth()- Depth_Keller msg: 1730867908.62 epoch sec; seqNo:474; depth 32.71 2024-11-06T04:38:28Z- handleDVL() - DVL msg epochMillisec: 1730867908.64 2024-11-06T04:38:28Z- handleDvl() - RDI_Pathfinder msg: 1730867908.64 sec; seqNo:156; #beams:4 2024-11-06T04:38:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 67.16 , 55.58 , 54.42 , 70.05 2024-11-06T04:38:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.11 , 0.00 2024-11-06T04:38:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867908.66 epoch sec; seqNo:474; 4061581.65 north; 594339.01 east; 36.70 lat; -121.94 lon; 32.71 depth; utmZone: 10 2024-11-06T04:38:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867905.41 2024-11-06T04:38:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:28Z- waiting for 1730867909.41, time is 1730867908.66 2024-11-06T04:38:29Z- AHRS_M2 msg: 1730867909.01 epoch sec; seqNo:437 2024-11-06T04:38:29Z- AHRS_M2 msg: 0.14 phi; -0.23 theta; 5.52 psi 2024-11-06T04:38:29Z- handleDepth()- Depth_Keller msg: 1730867909.01 epoch sec; seqNo:475; depth 32.79 2024-11-06T04:38:29Z- handleDVL() - DVL msg epochMillisec: 1730867909.02 2024-11-06T04:38:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:38:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:38:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:38:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:38:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:38:29Z- handleDvl() - RDI_Pathfinder msg: 1730867909.02 sec; seqNo:157; #beams:0 2024-11-06T04:38:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:38:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.11 , 0.00 2024-11-06T04:38:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867909.03 epoch sec; seqNo:475; 4061582.03 north; 594339.01 east; 36.70 lat; -121.94 lon; 32.79 depth; utmZone: 10 2024-11-06T04:38:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867905.41 2024-11-06T04:38:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:29Z- waiting for 1730867909.41, time is 1730867909.03 2024-11-06T04:38:29Z- AHRS_M2 msg: 1730867909.41 epoch sec; seqNo:438 2024-11-06T04:38:29Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.52 psi 2024-11-06T04:38:29Z- handleDepth()- Depth_Keller msg: 1730867909.42 epoch sec; seqNo:476; depth 32.88 2024-11-06T04:38:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867909.45 epoch sec; seqNo:476; 4061582.02 north; 594337.79 east; 36.70 lat; -121.94 lon; 32.88 depth; utmZone: 10 2024-11-06T04:38:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867905.41 2024-11-06T04:38:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:29Z- waiting for 1730867909.41, time is 1730867909.45 2024-11-06T04:38:29Z- LcmTrn::updateTrn() >>>> pose time 1730867909.45, meas time 1730867909.02 <<<< 2024-11-06T04:38:29Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867909.45 2024-11-06T04:38:29Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:38:29Z- TerrainNav::motionUpdate() - currEstimate = (4061582, 594337.79, 32.88) 2024-11-06T04:38:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:38:29Z- TerrainNav:: SubcloudNIS is 0.22, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:38:29Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:38:29Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:38:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:38:29Z- variances([0], [2], and psi_berg) = 0.055420 0.050517 0.000000 2024-11-06T04:38:29Z- publishEstimatesDecoder() - at 1730867909.45 sec 2024-11-06T04:38:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:38:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061526.31 594341.80 42.66 2024-11-06T04:38:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061525.99 594341.89 42.11 lat/lon: 36.69 -121.94 2024-11-06T04:38:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.06 0.05 1.11 0.00 2024-11-06T04:38:29Z- AHRS_M2 msg: 1730867909.82 epoch sec; seqNo:439 2024-11-06T04:38:29Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.52 psi 2024-11-06T04:38:29Z- handleDepth()- Depth_Keller msg: 1730867909.83 epoch sec; seqNo:477; depth 32.95 2024-11-06T04:38:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867909.84 epoch sec; seqNo:477; 4061582.40 north; 594337.78 east; 36.70 lat; -121.94 lon; 32.95 depth; utmZone: 10 2024-11-06T04:38:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867909.45 2024-11-06T04:38:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:29Z- waiting for 1730867913.45, time is 1730867909.84 2024-11-06T04:38:30Z- AHRS_M2 msg: 1730867910.24 epoch sec; seqNo:440 2024-11-06T04:38:30Z- AHRS_M2 msg: 0.13 phi; -0.23 theta; 5.52 psi 2024-11-06T04:38:30Z- handleDepth()- Depth_Keller msg: 1730867910.24 epoch sec; seqNo:478; depth 33.04 2024-11-06T04:38:30Z- handleDVL() - DVL msg epochMillisec: 1730867910.25 2024-11-06T04:38:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:38:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:38:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:38:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:38:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:38:30Z- handleDvl() - RDI_Pathfinder msg: 1730867910.25 sec; seqNo:158; #beams:0 2024-11-06T04:38:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:38:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.11 , 0.00 2024-11-06T04:38:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867910.26 epoch sec; seqNo:478; 4061582.40 north; 594337.78 east; 36.70 lat; -121.94 lon; 33.04 depth; utmZone: 10 2024-11-06T04:38:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867909.45 2024-11-06T04:38:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:30Z- waiting for 1730867913.45, time is 1730867910.26 2024-11-06T04:38:30Z- AHRS_M2 msg: 1730867910.62 epoch sec; seqNo:441 2024-11-06T04:38:30Z- AHRS_M2 msg: 0.13 phi; -0.22 theta; 5.52 psi 2024-11-06T04:38:30Z- handleDepth()- Depth_Keller msg: 1730867910.63 epoch sec; seqNo:479; depth 33.11 2024-11-06T04:38:30Z- handleDVL() - DVL msg epochMillisec: 1730867910.68 2024-11-06T04:38:30Z- handleDvl() - RDI_Pathfinder msg: 1730867910.68 sec; seqNo:159; #beams:4 2024-11-06T04:38:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 66.28 , 55.28 , 54.13 , 68.60 2024-11-06T04:38:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.08 , 0.00 2024-11-06T04:38:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867910.70 epoch sec; seqNo:479; 4061582.78 north; 594337.78 east; 36.70 lat; -121.94 lon; 33.11 depth; utmZone: 10 2024-11-06T04:38:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867909.45 2024-11-06T04:38:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:30Z- waiting for 1730867913.45, time is 1730867910.70 2024-11-06T04:38:31Z- AHRS_M2 msg: 1730867911.03 epoch sec; seqNo:442 2024-11-06T04:38:31Z- AHRS_M2 msg: 0.13 phi; -0.22 theta; 5.53 psi 2024-11-06T04:38:31Z- handleDepth()- Depth_Keller msg: 1730867911.03 epoch sec; seqNo:480; depth 33.21 2024-11-06T04:38:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867911.05 epoch sec; seqNo:480; 4061583.15 north; 594337.77 east; 36.70 lat; -121.94 lon; 33.21 depth; utmZone: 10 2024-11-06T04:38:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867909.45 2024-11-06T04:38:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:31Z- waiting for 1730867913.45, time is 1730867911.05 2024-11-06T04:38:31Z- AHRS_M2 msg: 1730867911.43 epoch sec; seqNo:443 2024-11-06T04:38:31Z- AHRS_M2 msg: 0.13 phi; -0.22 theta; 5.53 psi 2024-11-06T04:38:31Z- handleDepth()- Depth_Keller msg: 1730867911.44 epoch sec; seqNo:481; depth 33.27 2024-11-06T04:38:31Z- handleDVL() - DVL msg epochMillisec: 1730867911.44 2024-11-06T04:38:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:38:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:38:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:38:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:38:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:38:31Z- handleDvl() - RDI_Pathfinder msg: 1730867911.44 sec; seqNo:160; #beams:0 2024-11-06T04:38:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:38:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.08 , 0.00 2024-11-06T04:38:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867911.45 epoch sec; seqNo:481; 4061583.14 north; 594336.55 east; 36.70 lat; -121.94 lon; 33.27 depth; utmZone: 10 2024-11-06T04:38:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867909.45 2024-11-06T04:38:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:31Z- waiting for 1730867913.45, time is 1730867911.45 2024-11-06T04:38:31Z- AHRS_M2 msg: 1730867911.84 epoch sec; seqNo:444 2024-11-06T04:38:31Z- AHRS_M2 msg: 0.13 phi; -0.22 theta; 5.53 psi 2024-11-06T04:38:31Z- handleDepth()- Depth_Keller msg: 1730867911.85 epoch sec; seqNo:482; depth 33.35 2024-11-06T04:38:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867911.86 epoch sec; seqNo:482; 4061583.52 north; 594336.55 east; 36.70 lat; -121.94 lon; 33.35 depth; utmZone: 10 2024-11-06T04:38:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867909.45 2024-11-06T04:38:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:32Z- waiting for 1730867913.45, time is 1730867911.86 2024-11-06T04:38:32Z- AHRS_M2 msg: 1730867912.24 epoch sec; seqNo:445 2024-11-06T04:38:32Z- AHRS_M2 msg: 0.13 phi; -0.21 theta; 5.53 psi 2024-11-06T04:38:32Z- handleDepth()- Depth_Keller msg: 1730867912.25 epoch sec; seqNo:483; depth 33.41 2024-11-06T04:38:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867912.27 epoch sec; seqNo:483; 4061583.90 north; 594336.55 east; 36.70 lat; -121.94 lon; 33.41 depth; utmZone: 10 2024-11-06T04:38:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867909.45 2024-11-06T04:38:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:32Z- waiting for 1730867913.45, time is 1730867912.27 2024-11-06T04:38:32Z- AHRS_M2 msg: 1730867912.65 epoch sec; seqNo:446 2024-11-06T04:38:32Z- AHRS_M2 msg: 0.13 phi; -0.21 theta; 5.53 psi 2024-11-06T04:38:32Z- handleDepth()- Depth_Keller msg: 1730867912.65 epoch sec; seqNo:484; depth 33.50 2024-11-06T04:38:32Z- handleDVL() - DVL msg epochMillisec: 1730867912.68 2024-11-06T04:38:32Z- handleDvl() - RDI_Pathfinder msg: 1730867912.68 sec; seqNo:161; #beams:4 2024-11-06T04:38:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.41 , 54.99 , 53.84 , 67.73 2024-11-06T04:38:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , -0.03 2024-11-06T04:38:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867912.71 epoch sec; seqNo:484; 4061583.90 north; 594336.55 east; 36.70 lat; -121.94 lon; 33.50 depth; utmZone: 10 2024-11-06T04:38:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867909.45 2024-11-06T04:38:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:32Z- waiting for 1730867913.45, time is 1730867912.71 2024-11-06T04:38:33Z- AHRS_M2 msg: 1730867913.05 epoch sec; seqNo:447 2024-11-06T04:38:33Z- AHRS_M2 msg: 0.13 phi; -0.21 theta; 5.53 psi 2024-11-06T04:38:33Z- handleDepth()- Depth_Keller msg: 1730867913.06 epoch sec; seqNo:485; depth 33.57 2024-11-06T04:38:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867913.08 epoch sec; seqNo:485; 4061584.26 north; 594335.32 east; 36.70 lat; -121.94 lon; 33.57 depth; utmZone: 10 2024-11-06T04:38:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867909.45 2024-11-06T04:38:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:33Z- waiting for 1730867913.45, time is 1730867913.08 2024-11-06T04:38:33Z- AHRS_M2 msg: 1730867913.45 epoch sec; seqNo:448 2024-11-06T04:38:33Z- AHRS_M2 msg: 0.13 phi; -0.20 theta; 5.53 psi 2024-11-06T04:38:33Z- handleDepth()- Depth_Keller msg: 1730867913.46 epoch sec; seqNo:486; depth 33.63 2024-11-06T04:38:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867913.47 epoch sec; seqNo:486; 4061584.64 north; 594335.32 east; 36.70 lat; -121.94 lon; 33.63 depth; utmZone: 10 2024-11-06T04:38:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867909.45 2024-11-06T04:38:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:33Z- waiting for 1730867913.45, time is 1730867913.47 2024-11-06T04:38:33Z- LcmTrn::updateTrn() >>>> pose time 1730867913.47, meas time 1730867912.68 <<<< 2024-11-06T04:38:33Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867913.47 2024-11-06T04:38:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867912.68 sec, ping # 0; waiting for INS data more recent than 1730867909.45... 2024-11-06T04:38:33Z- TerrainNav::motionUpdate() - currEstimate = (4061585, 594335.32, 33.63) 2024-11-06T04:38:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:38:33Z- TerrainNav:: SubcloudNIS is 0.22, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:38:33Z- TNavPF::Projecting Measurements 2024-11-06T04:38:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:38:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:38:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:38:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:38:33Z- TNavFilter::4 good beams. 2024-11-06T04:38:33Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:38:33Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:38:33Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:38:33Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:38:33Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:38:33Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:38:33Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:38:33Z- TerrainMapOctree: (vehN, vehE ) = (4061584.13, 594335.80). 2024-11-06T04:38:33Z- TerrainMapOctree: Min Distance = 186.74. 2024-11-06T04:38:33Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:38:33Z- Forcing Subcloud Comparison 2024-11-06T04:38:33Z- Weighting particles with subcloud comparison 2024-11-06T04:38:33Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:33Z- meanExpectedMeasDiff: -0.311923 delta_rms_squared: 2.299915 alpha: 0.991379 2024-11-06T04:38:33Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:33Z- meanExpectedMeasDiff: 0.829462 delta_rms_squared: 1.653815 alpha: 0.988051 2024-11-06T04:38:33Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:33Z- meanExpectedMeasDiff: 0.893026 delta_rms_squared: 1.516622 alpha: 0.986984 2024-11-06T04:38:33Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:33Z- meanExpectedMeasDiff: 1.166430 delta_rms_squared: 2.502743 alpha: 0.992072 2024-11-06T04:38:33Z- Subcloud NIS: 0.197964 2024-11-06T04:38:33Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:38:33Z- TNavPF:: sumWeights = 0.613548 2024-11-06T04:38:33Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:38:33Z- TNavPF::First Term of Map Covariance Matrix is 0.72 2024-11-06T04:38:33Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.02 2024-11-06T04:38:33Z- TerrainNav::Resampling particle filter... 2024-11-06T04:38:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867912.68 sec. 2024-11-06T04:38:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:38:33Z- variances([0], [2], and psi_berg) = 0.057994 0.052721 0.000000 2024-11-06T04:38:33Z- publishEstimatesDecoder() - at 1730867913.47 sec 2024-11-06T04:38:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:38:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061528.40 594339.06 43.14 2024-11-06T04:38:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061528.61 594339.42 42.60 lat/lon: 36.69 -121.94 2024-11-06T04:38:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.06 0.05 0.91 0.00 2024-11-06T04:38:33Z- AHRS_M2 msg: 1730867913.87 epoch sec; seqNo:449 2024-11-06T04:38:33Z- AHRS_M2 msg: 0.13 phi; -0.20 theta; 5.53 psi 2024-11-06T04:38:33Z- handleDepth()- Depth_Keller msg: 1730867913.88 epoch sec; seqNo:487; depth 33.70 2024-11-06T04:38:33Z- handleDVL() - DVL msg epochMillisec: 1730867913.88 2024-11-06T04:38:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:38:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:38:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:38:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:38:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:38:33Z- handleDvl() - RDI_Pathfinder msg: 1730867913.88 sec; seqNo:162; #beams:0 2024-11-06T04:38:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:38:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , -0.03 2024-11-06T04:38:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867913.90 epoch sec; seqNo:487; 4061584.64 north; 594335.32 east; 36.70 lat; -121.94 lon; 33.70 depth; utmZone: 10 2024-11-06T04:38:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867913.47 2024-11-06T04:38:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:34Z- waiting for 1730867917.47, time is 1730867913.90 2024-11-06T04:38:34Z- AHRS_M2 msg: 1730867914.26 epoch sec; seqNo:450 2024-11-06T04:38:34Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.53 psi 2024-11-06T04:38:34Z- handleDepth()- Depth_Keller msg: 1730867914.27 epoch sec; seqNo:488; depth 33.77 2024-11-06T04:38:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867914.28 epoch sec; seqNo:488; 4061585.02 north; 594335.31 east; 36.70 lat; -121.94 lon; 33.77 depth; utmZone: 10 2024-11-06T04:38:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867913.47 2024-11-06T04:38:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:34Z- waiting for 1730867917.47, time is 1730867914.28 2024-11-06T04:38:34Z- AHRS_M2 msg: 1730867914.66 epoch sec; seqNo:451 2024-11-06T04:38:34Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.53 psi 2024-11-06T04:38:34Z- handleDepth()- Depth_Keller msg: 1730867914.67 epoch sec; seqNo:489; depth 33.84 2024-11-06T04:38:34Z- handleDVL() - DVL msg epochMillisec: 1730867914.70 2024-11-06T04:38:34Z- handleDvl() - RDI_Pathfinder msg: 1730867914.70 sec; seqNo:163; #beams:4 2024-11-06T04:38:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.55 , 54.42 , 53.26 , 66.57 2024-11-06T04:38:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.11 , -0.06 2024-11-06T04:38:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867914.72 epoch sec; seqNo:489; 4061585.40 north; 594335.31 east; 36.70 lat; -121.94 lon; 33.84 depth; utmZone: 10 2024-11-06T04:38:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867913.47 2024-11-06T04:38:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:34Z- waiting for 1730867917.47, time is 1730867914.72 2024-11-06T04:38:35Z- AHRS_M2 msg: 1730867915.07 epoch sec; seqNo:452 2024-11-06T04:38:35Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.52 psi 2024-11-06T04:38:35Z- handleDepth()- Depth_Keller msg: 1730867915.07 epoch sec; seqNo:490; depth 33.91 2024-11-06T04:38:35Z- handleDVL() - DVL msg epochMillisec: 1730867915.08 2024-11-06T04:38:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:38:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:38:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:38:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:38:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:38:35Z- handleDvl() - RDI_Pathfinder msg: 1730867915.08 sec; seqNo:164; #beams:0 2024-11-06T04:38:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:38:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.11 , -0.06 2024-11-06T04:38:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867915.09 epoch sec; seqNo:490; 4061585.39 north; 594334.09 east; 36.70 lat; -121.94 lon; 33.91 depth; utmZone: 10 2024-11-06T04:38:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867913.47 2024-11-06T04:38:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:35Z- waiting for 1730867917.47, time is 1730867915.09 2024-11-06T04:38:35Z- AHRS_M2 msg: 1730867915.47 epoch sec; seqNo:453 2024-11-06T04:38:35Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.52 psi 2024-11-06T04:38:35Z- handleDepth()- Depth_Keller msg: 1730867915.48 epoch sec; seqNo:491; depth 33.97 2024-11-06T04:38:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867915.49 epoch sec; seqNo:491; 4061585.77 north; 594334.08 east; 36.70 lat; -121.94 lon; 33.97 depth; utmZone: 10 2024-11-06T04:38:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867913.47 2024-11-06T04:38:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:35Z- waiting for 1730867917.47, time is 1730867915.49 2024-11-06T04:38:35Z- AHRS_M2 msg: 1730867915.88 epoch sec; seqNo:454 2024-11-06T04:38:35Z- AHRS_M2 msg: 0.11 phi; -0.19 theta; 5.52 psi 2024-11-06T04:38:35Z- handleDepth()- Depth_Keller msg: 1730867915.88 epoch sec; seqNo:492; depth 34.04 2024-11-06T04:38:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867915.90 epoch sec; seqNo:492; 4061586.15 north; 594334.08 east; 36.70 lat; -121.94 lon; 34.04 depth; utmZone: 10 2024-11-06T04:38:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867913.47 2024-11-06T04:38:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:36Z- waiting for 1730867917.47, time is 1730867915.90 2024-11-06T04:38:36Z- AHRS_M2 msg: 1730867916.28 epoch sec; seqNo:455 2024-11-06T04:38:36Z- AHRS_M2 msg: 0.11 phi; -0.19 theta; 5.52 psi 2024-11-06T04:38:36Z- handleDepth()- Depth_Keller msg: 1730867916.29 epoch sec; seqNo:493; depth 34.11 2024-11-06T04:38:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867916.30 epoch sec; seqNo:493; 4061586.15 north; 594334.08 east; 36.70 lat; -121.94 lon; 34.11 depth; utmZone: 10 2024-11-06T04:38:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867913.47 2024-11-06T04:38:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:36Z- waiting for 1730867917.47, time is 1730867916.30 2024-11-06T04:38:36Z- AHRS_M2 msg: 1730867916.69 epoch sec; seqNo:456 2024-11-06T04:38:36Z- AHRS_M2 msg: 0.11 phi; -0.19 theta; 5.51 psi 2024-11-06T04:38:36Z- handleDepth()- Depth_Keller msg: 1730867916.69 epoch sec; seqNo:494; depth 34.15 2024-11-06T04:38:36Z- handleDVL() - DVL msg epochMillisec: 1730867916.71 2024-11-06T04:38:36Z- handleDvl() - RDI_Pathfinder msg: 1730867916.71 sec; seqNo:165; #beams:4 2024-11-06T04:38:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.47 , 54.48 , 53.66 , 65.10 2024-11-06T04:38:36Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.17 , -0.00 2024-11-06T04:38:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867916.75 epoch sec; seqNo:494; 4061586.51 north; 594332.85 east; 36.70 lat; -121.94 lon; 34.15 depth; utmZone: 10 2024-11-06T04:38:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867913.47 2024-11-06T04:38:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:36Z- waiting for 1730867917.47, time is 1730867916.75 2024-11-06T04:38:37Z- AHRS_M2 msg: 1730867917.09 epoch sec; seqNo:457 2024-11-06T04:38:37Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.51 psi 2024-11-06T04:38:37Z- handleDepth()- Depth_Keller msg: 1730867917.10 epoch sec; seqNo:495; depth 34.25 2024-11-06T04:38:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867917.12 epoch sec; seqNo:495; 4061586.89 north; 594332.85 east; 36.70 lat; -121.94 lon; 34.25 depth; utmZone: 10 2024-11-06T04:38:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867913.47 2024-11-06T04:38:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:37Z- waiting for 1730867917.47, time is 1730867917.12 2024-11-06T04:38:37Z- AHRS_M2 msg: 1730867917.49 epoch sec; seqNo:458 2024-11-06T04:38:37Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.51 psi 2024-11-06T04:38:37Z- handleDepth()- Depth_Keller msg: 1730867917.50 epoch sec; seqNo:496; depth 34.31 2024-11-06T04:38:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867917.52 epoch sec; seqNo:496; 4061586.89 north; 594332.85 east; 36.70 lat; -121.94 lon; 34.31 depth; utmZone: 10 2024-11-06T04:38:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867913.47 2024-11-06T04:38:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:37Z- waiting for 1730867917.47, time is 1730867917.52 2024-11-06T04:38:37Z- LcmTrn::updateTrn() >>>> pose time 1730867917.52, meas time 1730867916.71 <<<< 2024-11-06T04:38:37Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867917.52 2024-11-06T04:38:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867916.71 sec, ping # 0; waiting for INS data more recent than 1730867913.47... 2024-11-06T04:38:37Z- TerrainNav::motionUpdate() - currEstimate = (4061587, 594332.85, 34.31) 2024-11-06T04:38:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:38:37Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:38:37Z- TNavPF::Projecting Measurements 2024-11-06T04:38:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:38:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:38:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:38:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:38:37Z- TNavFilter::4 good beams. 2024-11-06T04:38:37Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:38:37Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:38:37Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:38:37Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:38:37Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:38:37Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:38:37Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:38:37Z- TerrainMapOctree: (vehN, vehE ) = (4061586.44, 594333.34). 2024-11-06T04:38:37Z- TerrainMapOctree: Min Distance = 183.57. 2024-11-06T04:38:37Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:38:37Z- Forcing Subcloud Comparison 2024-11-06T04:38:37Z- Weighting particles with subcloud comparison 2024-11-06T04:38:37Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:37Z- meanExpectedMeasDiff: -0.521688 delta_rms_squared: 2.259366 alpha: 0.991226 2024-11-06T04:38:37Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:37Z- meanExpectedMeasDiff: 0.653145 delta_rms_squared: 1.421131 alpha: 0.986122 2024-11-06T04:38:37Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:37Z- meanExpectedMeasDiff: 0.852140 delta_rms_squared: 1.312556 alpha: 0.984991 2024-11-06T04:38:37Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:37Z- meanExpectedMeasDiff: 0.619352 delta_rms_squared: 2.043931 alpha: 0.990310 2024-11-06T04:38:37Z- Subcloud NIS: 0.174098 2024-11-06T04:38:37Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:38:37Z- TNavPF:: sumWeights = 0.647988 2024-11-06T04:38:37Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:38:37Z- TNavPF::First Term of Map Covariance Matrix is 0.72 2024-11-06T04:38:37Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T04:38:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867916.71 sec. 2024-11-06T04:38:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:38:37Z- variances([0], [2], and psi_berg) = 0.059643 0.054966 0.000000 2024-11-06T04:38:37Z- publishEstimatesDecoder() - at 1730867917.52 sec 2024-11-06T04:38:37Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:38:37Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061530.95 594337.01 42.99 2024-11-06T04:38:37Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061530.86 594336.95 43.12 lat/lon: 36.69 -121.94 2024-11-06T04:38:37Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.06 0.05 0.86 0.00 2024-11-06T04:38:37Z- AHRS_M2 msg: 1730867917.92 epoch sec; seqNo:459 2024-11-06T04:38:37Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.50 psi 2024-11-06T04:38:37Z- handleDepth()- Depth_Keller msg: 1730867917.93 epoch sec; seqNo:497; depth 34.38 2024-11-06T04:38:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867917.94 epoch sec; seqNo:497; 4061587.26 north; 594331.63 east; 36.70 lat; -121.94 lon; 34.38 depth; utmZone: 10 2024-11-06T04:38:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867917.52 2024-11-06T04:38:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:38Z- waiting for 1730867921.52, time is 1730867917.94 2024-11-06T04:38:38Z- AHRS_M2 msg: 1730867918.30 epoch sec; seqNo:460 2024-11-06T04:38:38Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.50 psi 2024-11-06T04:38:38Z- handleDepth()- Depth_Keller msg: 1730867918.31 epoch sec; seqNo:498; depth 34.45 2024-11-06T04:38:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867918.32 epoch sec; seqNo:498; 4061587.26 north; 594331.63 east; 36.70 lat; -121.94 lon; 34.45 depth; utmZone: 10 2024-11-06T04:38:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867917.52 2024-11-06T04:38:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:38Z- waiting for 1730867921.52, time is 1730867918.32 2024-11-06T04:38:38Z- AHRS_M2 msg: 1730867918.70 epoch sec; seqNo:461 2024-11-06T04:38:38Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.50 psi 2024-11-06T04:38:38Z- handleDepth()- Depth_Keller msg: 1730867918.71 epoch sec; seqNo:499; depth 34.50 2024-11-06T04:38:38Z- handleDVL() - DVL msg epochMillisec: 1730867918.74 2024-11-06T04:38:38Z- handleDvl() - RDI_Pathfinder msg: 1730867918.74 sec; seqNo:166; #beams:4 2024-11-06T04:38:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.92 , 53.93 , 53.66 , 64.01 2024-11-06T04:38:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.18 , 0.07 2024-11-06T04:38:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867918.75 epoch sec; seqNo:499; 4061587.63 north; 594331.62 east; 36.70 lat; -121.94 lon; 34.50 depth; utmZone: 10 2024-11-06T04:38:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867917.52 2024-11-06T04:38:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:38Z- waiting for 1730867921.52, time is 1730867918.75 2024-11-06T04:38:39Z- AHRS_M2 msg: 1730867919.11 epoch sec; seqNo:462 2024-11-06T04:38:39Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.50 psi 2024-11-06T04:38:39Z- handleDepth()- Depth_Keller msg: 1730867919.12 epoch sec; seqNo:500; depth 34.57 2024-11-06T04:38:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867919.13 epoch sec; seqNo:500; 4061588.01 north; 594331.62 east; 36.70 lat; -121.94 lon; 34.57 depth; utmZone: 10 2024-11-06T04:38:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867917.52 2024-11-06T04:38:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:39Z- waiting for 1730867921.52, time is 1730867919.13 2024-11-06T04:38:39Z- AHRS_M2 msg: 1730867919.51 epoch sec; seqNo:463 2024-11-06T04:38:39Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.50 psi 2024-11-06T04:38:39Z- handleDepth()- Depth_Keller msg: 1730867919.52 epoch sec; seqNo:501; depth 34.63 2024-11-06T04:38:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867919.53 epoch sec; seqNo:501; 4061588.00 north; 594330.40 east; 36.70 lat; -121.94 lon; 34.63 depth; utmZone: 10 2024-11-06T04:38:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867917.52 2024-11-06T04:38:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:39Z- waiting for 1730867921.52, time is 1730867919.53 2024-11-06T04:38:39Z- AHRS_M2 msg: 1730867919.91 epoch sec; seqNo:464 2024-11-06T04:38:39Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.50 psi 2024-11-06T04:38:39Z- handleDepth()- Depth_Keller msg: 1730867919.93 epoch sec; seqNo:502; depth 34.69 2024-11-06T04:38:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867919.94 epoch sec; seqNo:502; 4061588.38 north; 594330.39 east; 36.70 lat; -121.94 lon; 34.69 depth; utmZone: 10 2024-11-06T04:38:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867917.52 2024-11-06T04:38:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:40Z- waiting for 1730867921.52, time is 1730867919.94 2024-11-06T04:38:40Z- AHRS_M2 msg: 1730867920.32 epoch sec; seqNo:465 2024-11-06T04:38:40Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.50 psi 2024-11-06T04:38:40Z- handleDepth()- Depth_Keller msg: 1730867920.34 epoch sec; seqNo:503; depth 34.75 2024-11-06T04:38:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867920.41 epoch sec; seqNo:503; 4061588.38 north; 594330.39 east; 36.70 lat; -121.94 lon; 34.75 depth; utmZone: 10 2024-11-06T04:38:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867917.52 2024-11-06T04:38:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:40Z- waiting for 1730867921.52, time is 1730867920.41 2024-11-06T04:38:40Z- AHRS_M2 msg: 1730867920.72 epoch sec; seqNo:466 2024-11-06T04:38:40Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.50 psi 2024-11-06T04:38:40Z- handleDepth()- Depth_Keller msg: 1730867920.73 epoch sec; seqNo:504; depth 34.81 2024-11-06T04:38:40Z- handleDVL() - DVL msg epochMillisec: 1730867920.76 2024-11-06T04:38:40Z- handleDvl() - RDI_Pathfinder msg: 1730867920.76 sec; seqNo:167; #beams:4 2024-11-06T04:38:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.92 , 53.66 , 53.38 , 63.73 2024-11-06T04:38:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.11 , 0.08 2024-11-06T04:38:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867920.79 epoch sec; seqNo:504; 4061588.76 north; 594330.39 east; 36.70 lat; -121.94 lon; 34.81 depth; utmZone: 10 2024-11-06T04:38:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867917.52 2024-11-06T04:38:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:40Z- waiting for 1730867921.52, time is 1730867920.79 2024-11-06T04:38:41Z- AHRS_M2 msg: 1730867921.14 epoch sec; seqNo:467 2024-11-06T04:38:41Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.50 psi 2024-11-06T04:38:41Z- handleDepth()- Depth_Keller msg: 1730867921.15 epoch sec; seqNo:505; depth 34.88 2024-11-06T04:38:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867921.16 epoch sec; seqNo:505; 4061588.74 north; 594329.17 east; 36.70 lat; -121.94 lon; 34.88 depth; utmZone: 10 2024-11-06T04:38:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867917.52 2024-11-06T04:38:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:41Z- waiting for 1730867921.52, time is 1730867921.16 2024-11-06T04:38:41Z- AHRS_M2 msg: 1730867921.53 epoch sec; seqNo:468 2024-11-06T04:38:41Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.50 psi 2024-11-06T04:38:41Z- handleDepth()- Depth_Keller msg: 1730867921.54 epoch sec; seqNo:506; depth 34.94 2024-11-06T04:38:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867921.56 epoch sec; seqNo:506; 4061589.12 north; 594329.16 east; 36.70 lat; -121.94 lon; 34.94 depth; utmZone: 10 2024-11-06T04:38:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867917.52 2024-11-06T04:38:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:41Z- waiting for 1730867921.52, time is 1730867921.56 2024-11-06T04:38:41Z- LcmTrn::updateTrn() >>>> pose time 1730867921.56, meas time 1730867920.76 <<<< 2024-11-06T04:38:41Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867921.56 2024-11-06T04:38:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867920.76 sec, ping # 0; waiting for INS data more recent than 1730867917.52... 2024-11-06T04:38:41Z- TerrainNav::motionUpdate() - currEstimate = (4061589, 594329.16, 34.94) 2024-11-06T04:38:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:38:41Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:38:41Z- TNavPF::Projecting Measurements 2024-11-06T04:38:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:38:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:38:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:38:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:38:41Z- TNavFilter::4 good beams. 2024-11-06T04:38:41Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:38:41Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:38:41Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:38:41Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:38:41Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:38:41Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:38:41Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:38:41Z- TerrainMapOctree: (vehN, vehE ) = (4061588.68, 594329.89). 2024-11-06T04:38:41Z- TerrainMapOctree: Min Distance = 180.02. 2024-11-06T04:38:41Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:38:41Z- Forcing Subcloud Comparison 2024-11-06T04:38:41Z- Weighting particles with subcloud comparison 2024-11-06T04:38:41Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:41Z- meanExpectedMeasDiff: -0.529122 delta_rms_squared: 1.773032 alpha: 0.988846 2024-11-06T04:38:41Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:41Z- meanExpectedMeasDiff: 0.496132 delta_rms_squared: 1.348129 alpha: 0.985381 2024-11-06T04:38:41Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:41Z- meanExpectedMeasDiff: 0.890872 delta_rms_squared: 1.260796 alpha: 0.984385 2024-11-06T04:38:41Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:41Z- meanExpectedMeasDiff: 0.369106 delta_rms_squared: 1.919553 alpha: 0.989688 2024-11-06T04:38:41Z- Subcloud NIS: 0.156973 2024-11-06T04:38:41Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:38:41Z- TNavPF:: sumWeights = 0.667980 2024-11-06T04:38:41Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:38:41Z- TNavPF::First Term of Map Covariance Matrix is 0.67 2024-11-06T04:38:41Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.02 2024-11-06T04:38:41Z- TerrainNav::Resampling particle filter... 2024-11-06T04:38:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867920.76 sec. 2024-11-06T04:38:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:38:41Z- variances([0], [2], and psi_berg) = 0.063088 0.056872 0.000000 2024-11-06T04:38:41Z- publishEstimatesDecoder() - at 1730867921.56 sec 2024-11-06T04:38:41Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:38:41Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061532.89 594332.96 43.07 2024-11-06T04:38:41Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061533.09 594333.27 43.60 lat/lon: 36.69 -121.94 2024-11-06T04:38:41Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.06 0.06 0.83 0.00 2024-11-06T04:38:41Z- AHRS_M2 msg: 1730867921.94 epoch sec; seqNo:469 2024-11-06T04:38:41Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.50 psi 2024-11-06T04:38:41Z- handleDepth()- Depth_Keller msg: 1730867921.95 epoch sec; seqNo:507; depth 34.99 2024-11-06T04:38:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867921.98 epoch sec; seqNo:507; 4061589.50 north; 594329.16 east; 36.70 lat; -121.94 lon; 34.99 depth; utmZone: 10 2024-11-06T04:38:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867921.56 2024-11-06T04:38:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:42Z- waiting for 1730867925.56, time is 1730867921.98 2024-11-06T04:38:42Z- AHRS_M2 msg: 1730867922.35 epoch sec; seqNo:470 2024-11-06T04:38:42Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.50 psi 2024-11-06T04:38:42Z- handleDepth()- Depth_Keller msg: 1730867922.37 epoch sec; seqNo:508; depth 35.06 2024-11-06T04:38:42Z- handleDVL() - DVL msg epochMillisec: 1730867922.43 2024-11-06T04:38:42Z- handleDvl() - RDI_Pathfinder msg: 1730867922.43 sec; seqNo:168; #beams:4 2024-11-06T04:38:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.92 , 53.66 , 53.66 , 63.46 2024-11-06T04:38:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.01 2024-11-06T04:38:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867922.45 epoch sec; seqNo:508; 4061589.50 north; 594329.16 east; 36.70 lat; -121.94 lon; 35.06 depth; utmZone: 10 2024-11-06T04:38:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867921.56 2024-11-06T04:38:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:42Z- waiting for 1730867925.56, time is 1730867922.45 2024-11-06T04:38:42Z- AHRS_M2 msg: 1730867922.75 epoch sec; seqNo:471 2024-11-06T04:38:42Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.51 psi 2024-11-06T04:38:42Z- handleDepth()- Depth_Keller msg: 1730867922.76 epoch sec; seqNo:509; depth 35.12 2024-11-06T04:38:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867922.78 epoch sec; seqNo:509; 4061589.88 north; 594329.16 east; 36.70 lat; -121.94 lon; 35.12 depth; utmZone: 10 2024-11-06T04:38:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867921.56 2024-11-06T04:38:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:42Z- waiting for 1730867925.56, time is 1730867922.78 2024-11-06T04:38:43Z- AHRS_M2 msg: 1730867923.15 epoch sec; seqNo:472 2024-11-06T04:38:43Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.51 psi 2024-11-06T04:38:43Z- handleDepth()- Depth_Keller msg: 1730867923.16 epoch sec; seqNo:510; depth 35.17 2024-11-06T04:38:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867923.17 epoch sec; seqNo:510; 4061589.87 north; 594327.94 east; 36.70 lat; -121.94 lon; 35.17 depth; utmZone: 10 2024-11-06T04:38:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867921.56 2024-11-06T04:38:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:43Z- waiting for 1730867925.56, time is 1730867923.17 2024-11-06T04:38:43Z- AHRS_M2 msg: 1730867923.56 epoch sec; seqNo:473 2024-11-06T04:38:43Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.51 psi 2024-11-06T04:38:43Z- handleDepth()- Depth_Keller msg: 1730867923.56 epoch sec; seqNo:511; depth 35.22 2024-11-06T04:38:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867923.57 epoch sec; seqNo:511; 4061590.25 north; 594327.93 east; 36.70 lat; -121.94 lon; 35.22 depth; utmZone: 10 2024-11-06T04:38:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867921.56 2024-11-06T04:38:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:43Z- waiting for 1730867925.56, time is 1730867923.57 2024-11-06T04:38:43Z- AHRS_M2 msg: 1730867923.96 epoch sec; seqNo:474 2024-11-06T04:38:43Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.51 psi 2024-11-06T04:38:43Z- handleDepth()- Depth_Keller msg: 1730867923.96 epoch sec; seqNo:512; depth 35.27 2024-11-06T04:38:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867923.98 epoch sec; seqNo:512; 4061590.62 north; 594327.93 east; 36.70 lat; -121.94 lon; 35.27 depth; utmZone: 10 2024-11-06T04:38:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867921.56 2024-11-06T04:38:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:44Z- waiting for 1730867925.56, time is 1730867923.98 2024-11-06T04:38:44Z- AHRS_M2 msg: 1730867924.41 epoch sec; seqNo:475 2024-11-06T04:38:44Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.51 psi 2024-11-06T04:38:44Z- handleDepth()- Depth_Keller msg: 1730867924.41 epoch sec; seqNo:513; depth 35.32 2024-11-06T04:38:44Z- handleDVL() - DVL msg epochMillisec: 1730867924.43 2024-11-06T04:38:44Z- handleDvl() - RDI_Pathfinder msg: 1730867924.43 sec; seqNo:169; #beams:4 2024-11-06T04:38:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.09 , 53.38 , 52.83 , 62.91 2024-11-06T04:38:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , -0.08 2024-11-06T04:38:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867924.44 epoch sec; seqNo:513; 4061590.62 north; 594327.93 east; 36.70 lat; -121.94 lon; 35.32 depth; utmZone: 10 2024-11-06T04:38:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867921.56 2024-11-06T04:38:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:44Z- waiting for 1730867925.56, time is 1730867924.44 2024-11-06T04:38:44Z- AHRS_M2 msg: 1730867924.77 epoch sec; seqNo:476 2024-11-06T04:38:44Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.51 psi 2024-11-06T04:38:44Z- handleDepth()- Depth_Keller msg: 1730867924.78 epoch sec; seqNo:514; depth 35.38 2024-11-06T04:38:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867924.79 epoch sec; seqNo:514; 4061590.99 north; 594326.70 east; 36.70 lat; -121.94 lon; 35.38 depth; utmZone: 10 2024-11-06T04:38:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867921.56 2024-11-06T04:38:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:44Z- waiting for 1730867925.56, time is 1730867924.79 2024-11-06T04:38:45Z- AHRS_M2 msg: 1730867925.19 epoch sec; seqNo:477 2024-11-06T04:38:45Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.50 psi 2024-11-06T04:38:45Z- handleDepth()- Depth_Keller msg: 1730867925.23 epoch sec; seqNo:515; depth 35.43 2024-11-06T04:38:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867925.25 epoch sec; seqNo:515; 4061591.37 north; 594326.70 east; 36.70 lat; -121.94 lon; 35.43 depth; utmZone: 10 2024-11-06T04:38:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867921.56 2024-11-06T04:38:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:45Z- waiting for 1730867925.56, time is 1730867925.25 2024-11-06T04:38:45Z- AHRS_M2 msg: 1730867925.57 epoch sec; seqNo:478 2024-11-06T04:38:45Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.50 psi 2024-11-06T04:38:45Z- handleDepth()- Depth_Keller msg: 1730867925.58 epoch sec; seqNo:516; depth 35.50 2024-11-06T04:38:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867925.59 epoch sec; seqNo:516; 4061591.37 north; 594326.70 east; 36.70 lat; -121.94 lon; 35.50 depth; utmZone: 10 2024-11-06T04:38:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867921.56 2024-11-06T04:38:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:45Z- waiting for 1730867925.56, time is 1730867925.59 2024-11-06T04:38:45Z- LcmTrn::updateTrn() >>>> pose time 1730867925.59, meas time 1730867924.43 <<<< 2024-11-06T04:38:45Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867925.59 2024-11-06T04:38:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867924.43 sec, ping # 0; waiting for INS data more recent than 1730867921.56... 2024-11-06T04:38:45Z- TerrainNav::motionUpdate() - currEstimate = (4061591, 594326.70, 35.50) 2024-11-06T04:38:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:38:45Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:38:45Z- TNavPF::Projecting Measurements 2024-11-06T04:38:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:38:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:38:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:38:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:38:45Z- TNavFilter::4 good beams. 2024-11-06T04:38:45Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:38:45Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:38:45Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:38:45Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:38:45Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:38:45Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:38:45Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:38:45Z- TerrainMapOctree: (vehN, vehE ) = (4061590.73, 594327.40). 2024-11-06T04:38:45Z- TerrainMapOctree: Min Distance = 177.09. 2024-11-06T04:38:45Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:38:45Z- Forcing Subcloud Comparison 2024-11-06T04:38:45Z- Weighting particles with subcloud comparison 2024-11-06T04:38:45Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:45Z- meanExpectedMeasDiff: -0.541217 delta_rms_squared: 1.706777 alpha: 0.988418 2024-11-06T04:38:45Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:45Z- meanExpectedMeasDiff: 0.440541 delta_rms_squared: 1.101106 alpha: 0.982160 2024-11-06T04:38:45Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:45Z- meanExpectedMeasDiff: 0.739767 delta_rms_squared: 1.226840 alpha: 0.983959 2024-11-06T04:38:45Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:38:45Z- meanExpectedMeasDiff: 0.222317 delta_rms_squared: 1.856991 alpha: 0.989345 2024-11-06T04:38:45Z- Subcloud NIS: 0.148553 2024-11-06T04:38:45Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:38:45Z- TNavPF:: sumWeights = 0.685954 2024-11-06T04:38:45Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:38:45Z- TNavPF::First Term of Map Covariance Matrix is 0.68 2024-11-06T04:38:45Z- TNavPF::Calculated NIS Value : 0.09 numBeams normalized NIS: 0.02 2024-11-06T04:38:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867924.43 sec. 2024-11-06T04:38:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:38:45Z- variances([0], [2], and psi_berg) = 0.064429 0.058525 0.000000 2024-11-06T04:38:45Z- publishEstimatesDecoder() - at 1730867925.59 sec 2024-11-06T04:38:45Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:38:45Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061535.53 594330.00 44.04 2024-11-06T04:38:45Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061535.34 594330.80 44.06 lat/lon: 36.69 -121.94 2024-11-06T04:38:45Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.06 0.06 0.85 0.00 2024-11-06T04:38:45Z- AHRS_M2 msg: 1730867925.97 epoch sec; seqNo:479 2024-11-06T04:38:45Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.50 psi 2024-11-06T04:38:45Z- handleDepth()- Depth_Keller msg: 1730867925.98 epoch sec; seqNo:517; depth 35.53 2024-11-06T04:38:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867926.02 epoch sec; seqNo:517; 4061591.75 north; 594326.69 east; 36.70 lat; -121.94 lon; 35.53 depth; utmZone: 10 2024-11-06T04:38:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867925.59 2024-11-06T04:38:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:46Z- waiting for 1730867929.59, time is 1730867926.02 2024-11-06T04:38:46Z- AHRS_M2 msg: 1730867926.41 epoch sec; seqNo:480 2024-11-06T04:38:46Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.50 psi 2024-11-06T04:38:46Z- handleDepth()- Depth_Keller msg: 1730867926.42 epoch sec; seqNo:518; depth 35.60 2024-11-06T04:38:46Z- handleDVL() - DVL msg epochMillisec: 1730867926.45 2024-11-06T04:38:46Z- handleDvl() - RDI_Pathfinder msg: 1730867926.45 sec; seqNo:170; #beams:4 2024-11-06T04:38:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.55 , 53.38 , 52.83 , 62.64 2024-11-06T04:38:46Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.11 , -0.06 2024-11-06T04:38:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867926.46 epoch sec; seqNo:518; 4061592.13 north; 594326.69 east; 36.70 lat; -121.94 lon; 35.60 depth; utmZone: 10 2024-11-06T04:38:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867925.59 2024-11-06T04:38:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:46Z- waiting for 1730867929.59, time is 1730867926.46 2024-11-06T04:38:46Z- AHRS_M2 msg: 1730867926.78 epoch sec; seqNo:481 2024-11-06T04:38:46Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.49 psi 2024-11-06T04:38:46Z- handleDepth()- Depth_Keller msg: 1730867926.79 epoch sec; seqNo:519; depth 35.65 2024-11-06T04:38:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867926.82 epoch sec; seqNo:519; 4061592.11 north; 594325.47 east; 36.70 lat; -121.94 lon; 35.65 depth; utmZone: 10 2024-11-06T04:38:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867925.59 2024-11-06T04:38:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:46Z- waiting for 1730867929.59, time is 1730867926.82 2024-11-06T04:38:47Z- AHRS_M2 msg: 1730867927.19 epoch sec; seqNo:482 2024-11-06T04:38:47Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.49 psi 2024-11-06T04:38:47Z- handleDepth()- Depth_Keller msg: 1730867927.19 epoch sec; seqNo:520; depth 35.71 2024-11-06T04:38:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867927.20 epoch sec; seqNo:520; 4061592.49 north; 594325.47 east; 36.70 lat; -121.94 lon; 35.71 depth; utmZone: 10 2024-11-06T04:38:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867925.59 2024-11-06T04:38:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:47Z- waiting for 1730867929.59, time is 1730867927.20 2024-11-06T04:38:47Z- AHRS_M2 msg: 1730867927.59 epoch sec; seqNo:483 2024-11-06T04:38:47Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.48 psi 2024-11-06T04:38:47Z- handleDepth()- Depth_Keller msg: 1730867927.60 epoch sec; seqNo:521; depth 35.77 2024-11-06T04:38:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867927.61 epoch sec; seqNo:521; 4061592.87 north; 594325.46 east; 36.70 lat; -121.94 lon; 35.77 depth; utmZone: 10 2024-11-06T04:38:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867925.59 2024-11-06T04:38:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:47Z- waiting for 1730867929.59, time is 1730867927.61 2024-11-06T04:38:48Z- AHRS_M2 msg: 1730867927.99 epoch sec; seqNo:484 2024-11-06T04:38:48Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.48 psi 2024-11-06T04:38:48Z- handleDepth()- Depth_Keller msg: 1730867928.01 epoch sec; seqNo:522; depth 35.83 2024-11-06T04:38:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867928.02 epoch sec; seqNo:522; 4061592.86 north; 594324.24 east; 36.70 lat; -121.94 lon; 35.83 depth; utmZone: 10 2024-11-06T04:38:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867925.59 2024-11-06T04:38:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:48Z- waiting for 1730867929.59, time is 1730867928.02 2024-11-06T04:38:48Z- AHRS_M2 msg: 1730867928.42 epoch sec; seqNo:485 2024-11-06T04:38:48Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.48 psi 2024-11-06T04:38:48Z- handleDepth()- Depth_Keller msg: 1730867928.43 epoch sec; seqNo:523; depth 35.89 2024-11-06T04:38:48Z- handleDVL() - DVL msg epochMillisec: 1730867928.45 2024-11-06T04:38:48Z- handleDvl() - RDI_Pathfinder msg: 1730867928.45 sec; seqNo:171; #beams:4 2024-11-06T04:38:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.00 , 53.11 , 52.56 , 62.09 2024-11-06T04:38:48Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , -0.14 , 0.03 2024-11-06T04:38:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867928.47 epoch sec; seqNo:523; 4061593.24 north; 594324.24 east; 36.70 lat; -121.94 lon; 35.89 depth; utmZone: 10 2024-11-06T04:38:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867925.59 2024-11-06T04:38:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:48Z- waiting for 1730867929.59, time is 1730867928.47 2024-11-06T04:38:48Z- AHRS_M2 msg: 1730867928.81 epoch sec; seqNo:486 2024-11-06T04:38:48Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.48 psi 2024-11-06T04:38:48Z- handleDepth()- Depth_Keller msg: 1730867928.81 epoch sec; seqNo:524; depth 35.95 2024-11-06T04:38:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867928.83 epoch sec; seqNo:524; 4061593.61 north; 594324.23 east; 36.70 lat; -121.94 lon; 35.95 depth; utmZone: 10 2024-11-06T04:38:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867925.59 2024-11-06T04:38:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:48Z- waiting for 1730867929.59, time is 1730867928.83 2024-11-06T04:38:49Z- AHRS_M2 msg: 1730867929.21 epoch sec; seqNo:487 2024-11-06T04:38:49Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.48 psi 2024-11-06T04:38:49Z- handleDepth()- Depth_Keller msg: 1730867929.21 epoch sec; seqNo:525; depth 35.99 2024-11-06T04:38:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867929.23 epoch sec; seqNo:525; 4061593.61 north; 594324.23 east; 36.70 lat; -121.94 lon; 35.99 depth; utmZone: 10 2024-11-06T04:38:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867925.59 2024-11-06T04:38:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:49Z- waiting for 1730867929.59, time is 1730867929.23 2024-11-06T04:38:49Z- AHRS_M2 msg: 1730867929.61 epoch sec; seqNo:488 2024-11-06T04:38:49Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.47 psi 2024-11-06T04:38:49Z- handleDepth()- Depth_Keller msg: 1730867929.62 epoch sec; seqNo:526; depth 36.06 2024-11-06T04:38:49Z- handleDVL() - DVL msg epochMillisec: 1730867929.62 2024-11-06T04:38:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:38:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:38:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:38:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:38:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:38:49Z- handleDvl() - RDI_Pathfinder msg: 1730867929.62 sec; seqNo:172; #beams:0 2024-11-06T04:38:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:38:49Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , -0.14 , 0.03 2024-11-06T04:38:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867929.63 epoch sec; seqNo:526; 4061593.98 north; 594323.01 east; 36.70 lat; -121.94 lon; 36.06 depth; utmZone: 10 2024-11-06T04:38:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867925.59 2024-11-06T04:38:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:49Z- waiting for 1730867929.59, time is 1730867929.63 2024-11-06T04:38:49Z- LcmTrn::updateTrn() >>>> pose time 1730867929.63, meas time 1730867929.62 <<<< 2024-11-06T04:38:49Z- LcmTrn::updateTrn() equating pose 1730867929.62 and meas times 1730867929.62 2024-11-06T04:38:49Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867929.62 2024-11-06T04:38:49Z- TerrainNav::motionUpdate() - currEstimate = (4061594, 594323.01, 36.06) 2024-11-06T04:38:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:38:49Z- TerrainNav:: SubcloudNIS is 0.15, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:38:49Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:38:49Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:38:49Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:38:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:38:49Z- variances([0], [2], and psi_berg) = 0.067960 0.061441 0.000000 2024-11-06T04:38:49Z- publishEstimatesDecoder() - at 1730867929.62 sec 2024-11-06T04:38:49Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:38:49Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061538.11 594326.23 44.31 2024-11-06T04:38:49Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061537.95 594327.11 44.62 lat/lon: 36.69 -121.94 2024-11-06T04:38:49Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.07 0.06 1.10 0.00 2024-11-06T04:38:50Z- AHRS_M2 msg: 1730867930.02 epoch sec; seqNo:489 2024-11-06T04:38:50Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.47 psi 2024-11-06T04:38:50Z- handleDepth()- Depth_Keller msg: 1730867930.02 epoch sec; seqNo:527; depth 36.11 2024-11-06T04:38:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867930.04 epoch sec; seqNo:527; 4061594.36 north; 594323.00 east; 36.70 lat; -121.94 lon; 36.11 depth; utmZone: 10 2024-11-06T04:38:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867929.62 2024-11-06T04:38:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:50Z- waiting for 1730867933.62, time is 1730867930.04 2024-11-06T04:38:50Z- AHRS_M2 msg: 1730867930.42 epoch sec; seqNo:490 2024-11-06T04:38:50Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.47 psi 2024-11-06T04:38:50Z- handleDepth()- Depth_Keller msg: 1730867930.43 epoch sec; seqNo:528; depth 36.19 2024-11-06T04:38:50Z- handleDVL() - DVL msg epochMillisec: 1730867930.46 2024-11-06T04:38:50Z- handleDvl() - RDI_Pathfinder msg: 1730867930.46 sec; seqNo:173; #beams:4 2024-11-06T04:38:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.28 , 52.56 , 52.29 , 62.09 2024-11-06T04:38:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.15 , 0.05 2024-11-06T04:38:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867930.47 epoch sec; seqNo:528; 4061594.36 north; 594323.00 east; 36.70 lat; -121.94 lon; 36.19 depth; utmZone: 10 2024-11-06T04:38:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867929.62 2024-11-06T04:38:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:50Z- waiting for 1730867933.62, time is 1730867930.47 2024-11-06T04:38:50Z- AHRS_M2 msg: 1730867930.84 epoch sec; seqNo:491 2024-11-06T04:38:50Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.47 psi 2024-11-06T04:38:50Z- handleDepth()- Depth_Keller msg: 1730867930.85 epoch sec; seqNo:529; depth 36.25 2024-11-06T04:38:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867930.87 epoch sec; seqNo:529; 4061594.74 north; 594323.00 east; 36.70 lat; -121.94 lon; 36.25 depth; utmZone: 10 2024-11-06T04:38:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867929.62 2024-11-06T04:38:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:51Z- waiting for 1730867933.62, time is 1730867930.87 2024-11-06T04:38:51Z- AHRS_M2 msg: 1730867931.23 epoch sec; seqNo:492 2024-11-06T04:38:51Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.47 psi 2024-11-06T04:38:51Z- handleDepth()- Depth_Keller msg: 1730867931.24 epoch sec; seqNo:530; depth 36.32 2024-11-06T04:38:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867931.25 epoch sec; seqNo:530; 4061594.72 north; 594321.78 east; 36.70 lat; -121.94 lon; 36.32 depth; utmZone: 10 2024-11-06T04:38:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867929.62 2024-11-06T04:38:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:51Z- waiting for 1730867933.62, time is 1730867931.25 2024-11-06T04:38:51Z- AHRS_M2 msg: 1730867931.64 epoch sec; seqNo:493 2024-11-06T04:38:51Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.47 psi 2024-11-06T04:38:51Z- handleDepth()- Depth_Keller msg: 1730867931.65 epoch sec; seqNo:531; depth 36.36 2024-11-06T04:38:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867931.66 epoch sec; seqNo:531; 4061595.10 north; 594321.78 east; 36.70 lat; -121.94 lon; 36.36 depth; utmZone: 10 2024-11-06T04:38:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867929.62 2024-11-06T04:38:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:51Z- waiting for 1730867933.62, time is 1730867931.66 2024-11-06T04:38:52Z- AHRS_M2 msg: 1730867932.03 epoch sec; seqNo:494 2024-11-06T04:38:52Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.47 psi 2024-11-06T04:38:52Z- handleDepth()- Depth_Keller msg: 1730867932.05 epoch sec; seqNo:532; depth 36.42 2024-11-06T04:38:52Z- handleDVL() - DVL msg epochMillisec: 1730867932.05 2024-11-06T04:38:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:38:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:38:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:38:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:38:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:38:52Z- handleDvl() - RDI_Pathfinder msg: 1730867932.05 sec; seqNo:174; #beams:0 2024-11-06T04:38:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:38:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.15 , 0.05 2024-11-06T04:38:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867932.06 epoch sec; seqNo:532; 4061595.10 north; 594321.78 east; 36.70 lat; -121.94 lon; 36.42 depth; utmZone: 10 2024-11-06T04:38:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867929.62 2024-11-06T04:38:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:52Z- waiting for 1730867933.62, time is 1730867932.06 2024-11-06T04:38:52Z- AHRS_M2 msg: 1730867932.44 epoch sec; seqNo:495 2024-11-06T04:38:52Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.47 psi 2024-11-06T04:38:52Z- handleDepth()- Depth_Keller msg: 1730867932.45 epoch sec; seqNo:533; depth 36.49 2024-11-06T04:38:52Z- handleDVL() - DVL msg epochMillisec: 1730867932.48 2024-11-06T04:38:52Z- handleDvl() - RDI_Pathfinder msg: 1730867932.48 sec; seqNo:175; #beams:4 2024-11-06T04:38:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.27 , 52.56 , 52.01 , 62.09 2024-11-06T04:38:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.13 , 0.03 2024-11-06T04:38:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867932.51 epoch sec; seqNo:533; 4061595.47 north; 594320.55 east; 36.70 lat; -121.94 lon; 36.49 depth; utmZone: 10 2024-11-06T04:38:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867929.62 2024-11-06T04:38:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:52Z- waiting for 1730867933.62, time is 1730867932.51 2024-11-06T04:38:52Z- AHRS_M2 msg: 1730867932.85 epoch sec; seqNo:496 2024-11-06T04:38:52Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.47 psi 2024-11-06T04:38:52Z- handleDepth()- Depth_Keller msg: 1730867932.86 epoch sec; seqNo:534; depth 36.56 2024-11-06T04:38:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867932.87 epoch sec; seqNo:534; 4061595.85 north; 594320.55 east; 36.70 lat; -121.94 lon; 36.56 depth; utmZone: 10 2024-11-06T04:38:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867929.62 2024-11-06T04:38:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:53Z- waiting for 1730867933.62, time is 1730867932.87 2024-11-06T04:38:53Z- AHRS_M2 msg: 1730867933.25 epoch sec; seqNo:497 2024-11-06T04:38:53Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.47 psi 2024-11-06T04:38:53Z- handleDepth()- Depth_Keller msg: 1730867933.26 epoch sec; seqNo:535; depth 36.61 2024-11-06T04:38:53Z- handleDVL() - DVL msg epochMillisec: 1730867933.26 2024-11-06T04:38:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:38:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:38:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:38:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:38:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:38:53Z- handleDvl() - RDI_Pathfinder msg: 1730867933.26 sec; seqNo:176; #beams:0 2024-11-06T04:38:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:38:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.13 , 0.03 2024-11-06T04:38:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867933.28 epoch sec; seqNo:535; 4061595.85 north; 594320.55 east; 36.70 lat; -121.94 lon; 36.61 depth; utmZone: 10 2024-11-06T04:38:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867929.62 2024-11-06T04:38:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:53Z- waiting for 1730867933.62, time is 1730867933.28 2024-11-06T04:38:53Z- AHRS_M2 msg: 1730867933.65 epoch sec; seqNo:498 2024-11-06T04:38:53Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.47 psi 2024-11-06T04:38:53Z- handleDepth()- Depth_Keller msg: 1730867933.66 epoch sec; seqNo:536; depth 36.67 2024-11-06T04:38:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867933.67 epoch sec; seqNo:536; 4061596.23 north; 594320.54 east; 36.70 lat; -121.94 lon; 36.67 depth; utmZone: 10 2024-11-06T04:38:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867929.62 2024-11-06T04:38:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:53Z- waiting for 1730867933.62, time is 1730867933.67 2024-11-06T04:38:53Z- LcmTrn::updateTrn() >>>> pose time 1730867933.67, meas time 1730867933.26 <<<< 2024-11-06T04:38:53Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867933.67 2024-11-06T04:38:53Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:38:53Z- TerrainNav::motionUpdate() - currEstimate = (4061596, 594320.54, 36.67) 2024-11-06T04:38:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:38:53Z- TerrainNav:: SubcloudNIS is 0.15, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:38:53Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:38:53Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:38:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:38:53Z- variances([0], [2], and psi_berg) = 0.069706 0.063143 0.000000 2024-11-06T04:38:53Z- publishEstimatesDecoder() - at 1730867933.67 sec 2024-11-06T04:38:53Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:38:53Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061540.36 594323.84 45.35 2024-11-06T04:38:53Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061540.19 594324.65 45.24 lat/lon: 36.69 -121.94 2024-11-06T04:38:53Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.07 0.06 1.34 0.00 2024-11-06T04:38:54Z- AHRS_M2 msg: 1730867934.06 epoch sec; seqNo:499 2024-11-06T04:38:54Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.47 psi 2024-11-06T04:38:54Z- handleDepth()- Depth_Keller msg: 1730867934.07 epoch sec; seqNo:537; depth 36.74 2024-11-06T04:38:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867934.08 epoch sec; seqNo:537; 4061596.23 north; 594320.54 east; 36.70 lat; -121.94 lon; 36.74 depth; utmZone: 10 2024-11-06T04:38:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867933.67 2024-11-06T04:38:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:54Z- waiting for 1730867937.67, time is 1730867934.08 2024-11-06T04:38:54Z- AHRS_M2 msg: 1730867934.46 epoch sec; seqNo:500 2024-11-06T04:38:54Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.47 psi 2024-11-06T04:38:54Z- handleDepth()- Depth_Keller msg: 1730867934.47 epoch sec; seqNo:538; depth 36.80 2024-11-06T04:38:54Z- handleDVL() - DVL msg epochMillisec: 1730867934.50 2024-11-06T04:38:54Z- handleDvl() - RDI_Pathfinder msg: 1730867934.50 sec; seqNo:177; #beams:4 2024-11-06T04:38:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.27 , 52.28 , 51.74 , 62.09 2024-11-06T04:38:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.07 , 0.01 2024-11-06T04:38:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867934.52 epoch sec; seqNo:538; 4061596.59 north; 594319.32 east; 36.70 lat; -121.94 lon; 36.80 depth; utmZone: 10 2024-11-06T04:38:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867933.67 2024-11-06T04:38:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:54Z- waiting for 1730867937.67, time is 1730867934.52 2024-11-06T04:38:54Z- AHRS_M2 msg: 1730867934.86 epoch sec; seqNo:501 2024-11-06T04:38:54Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.47 psi 2024-11-06T04:38:54Z- handleDepth()- Depth_Keller msg: 1730867934.87 epoch sec; seqNo:539; depth 36.89 2024-11-06T04:38:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867934.88 epoch sec; seqNo:539; 4061596.59 north; 594319.32 east; 36.70 lat; -121.94 lon; 36.89 depth; utmZone: 10 2024-11-06T04:38:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867933.67 2024-11-06T04:38:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:55Z- waiting for 1730867937.67, time is 1730867934.88 2024-11-06T04:38:55Z- AHRS_M2 msg: 1730867935.27 epoch sec; seqNo:502 2024-11-06T04:38:55Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.47 psi 2024-11-06T04:38:55Z- handleDepth()- Depth_Keller msg: 1730867935.28 epoch sec; seqNo:540; depth 36.92 2024-11-06T04:38:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867935.29 epoch sec; seqNo:540; 4061596.97 north; 594319.31 east; 36.70 lat; -121.94 lon; 36.92 depth; utmZone: 10 2024-11-06T04:38:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867933.67 2024-11-06T04:38:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:55Z- waiting for 1730867937.67, time is 1730867935.29 2024-11-06T04:38:55Z- AHRS_M2 msg: 1730867935.67 epoch sec; seqNo:503 2024-11-06T04:38:55Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.47 psi 2024-11-06T04:38:55Z- handleDepth()- Depth_Keller msg: 1730867935.68 epoch sec; seqNo:541; depth 36.98 2024-11-06T04:38:55Z- handleDVL() - DVL msg epochMillisec: 1730867935.68 2024-11-06T04:38:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:38:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:38:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:38:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:38:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:38:55Z- handleDvl() - RDI_Pathfinder msg: 1730867935.68 sec; seqNo:178; #beams:0 2024-11-06T04:38:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:38:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.07 , 0.01 2024-11-06T04:38:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867935.69 epoch sec; seqNo:541; 4061597.35 north; 594319.31 east; 36.70 lat; -121.94 lon; 36.98 depth; utmZone: 10 2024-11-06T04:38:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867933.67 2024-11-06T04:38:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:55Z- waiting for 1730867937.67, time is 1730867935.69 2024-11-06T04:38:56Z- AHRS_M2 msg: 1730867936.08 epoch sec; seqNo:504 2024-11-06T04:38:56Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.47 psi 2024-11-06T04:38:56Z- handleDepth()- Depth_Keller msg: 1730867936.09 epoch sec; seqNo:542; depth 37.05 2024-11-06T04:38:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867936.11 epoch sec; seqNo:542; 4061597.34 north; 594318.09 east; 36.70 lat; -121.94 lon; 37.05 depth; utmZone: 10 2024-11-06T04:38:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867933.67 2024-11-06T04:38:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:56Z- waiting for 1730867937.67, time is 1730867936.11 2024-11-06T04:38:56Z- AHRS_M2 msg: 1730867936.48 epoch sec; seqNo:505 2024-11-06T04:38:56Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.47 psi 2024-11-06T04:38:56Z- handleDepth()- Depth_Keller msg: 1730867936.49 epoch sec; seqNo:543; depth 37.11 2024-11-06T04:38:56Z- handleDVL() - DVL msg epochMillisec: 1730867936.51 2024-11-06T04:38:56Z- handleDvl() - RDI_Pathfinder msg: 1730867936.51 sec; seqNo:179; #beams:4 2024-11-06T04:38:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.27 , 52.01 , 51.47 , 62.09 2024-11-06T04:38:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.06 , -0.01 2024-11-06T04:38:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867936.54 epoch sec; seqNo:543; 4061597.71 north; 594318.09 east; 36.70 lat; -121.94 lon; 37.11 depth; utmZone: 10 2024-11-06T04:38:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867933.67 2024-11-06T04:38:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:56Z- waiting for 1730867937.67, time is 1730867936.54 2024-11-06T04:38:56Z- AHRS_M2 msg: 1730867936.88 epoch sec; seqNo:506 2024-11-06T04:38:56Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.47 psi 2024-11-06T04:38:56Z- handleDepth()- Depth_Keller msg: 1730867936.89 epoch sec; seqNo:544; depth 37.19 2024-11-06T04:38:56Z- handleDVL() - DVL msg epochMillisec: 1730867936.89 2024-11-06T04:38:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:38:56Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:38:56Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:38:56Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:38:56Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:38:56Z- handleDvl() - RDI_Pathfinder msg: 1730867936.89 sec; seqNo:180; #beams:0 2024-11-06T04:38:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:38:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.06 , -0.01 2024-11-06T04:38:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867936.91 epoch sec; seqNo:544; 4061598.09 north; 594318.08 east; 36.70 lat; -121.94 lon; 37.19 depth; utmZone: 10 2024-11-06T04:38:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867933.67 2024-11-06T04:38:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:57Z- waiting for 1730867937.67, time is 1730867936.91 2024-11-06T04:38:57Z- AHRS_M2 msg: 1730867937.29 epoch sec; seqNo:507 2024-11-06T04:38:57Z- AHRS_M2 msg: 0.13 phi; -0.18 theta; 5.48 psi 2024-11-06T04:38:57Z- handleDepth()- Depth_Keller msg: 1730867937.30 epoch sec; seqNo:545; depth 37.25 2024-11-06T04:38:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867937.33 epoch sec; seqNo:545; 4061598.09 north; 594318.08 east; 36.70 lat; -121.94 lon; 37.25 depth; utmZone: 10 2024-11-06T04:38:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867933.67 2024-11-06T04:38:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:57Z- waiting for 1730867937.67, time is 1730867937.33 2024-11-06T04:38:57Z- AHRS_M2 msg: 1730867937.69 epoch sec; seqNo:508 2024-11-06T04:38:57Z- AHRS_M2 msg: 0.13 phi; -0.18 theta; 5.48 psi 2024-11-06T04:38:57Z- handleDepth()- Depth_Keller msg: 1730867937.70 epoch sec; seqNo:546; depth 37.31 2024-11-06T04:38:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867937.71 epoch sec; seqNo:546; 4061598.46 north; 594316.86 east; 36.70 lat; -121.94 lon; 37.31 depth; utmZone: 10 2024-11-06T04:38:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867933.67 2024-11-06T04:38:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:57Z- waiting for 1730867937.67, time is 1730867937.71 2024-11-06T04:38:57Z- LcmTrn::updateTrn() >>>> pose time 1730867937.71, meas time 1730867936.89 <<<< 2024-11-06T04:38:57Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867937.71 2024-11-06T04:38:57Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:38:57Z- TerrainNav::motionUpdate() - currEstimate = (4061598, 594316.86, 37.31) 2024-11-06T04:38:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:38:57Z- TerrainNav:: SubcloudNIS is 0.15, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:38:57Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:38:57Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:38:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:38:57Z- variances([0], [2], and psi_berg) = 0.073020 0.065966 0.000000 2024-11-06T04:38:57Z- publishEstimatesDecoder() - at 1730867937.71 sec 2024-11-06T04:38:57Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:38:57Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061542.61 594320.13 46.27 2024-11-06T04:38:57Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061542.43 594320.96 45.88 lat/lon: 36.69 -121.94 2024-11-06T04:38:57Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.07 0.07 1.59 0.00 2024-11-06T04:38:58Z- AHRS_M2 msg: 1730867938.09 epoch sec; seqNo:509 2024-11-06T04:38:58Z- AHRS_M2 msg: 0.13 phi; -0.18 theta; 5.48 psi 2024-11-06T04:38:58Z- handleDepth()- Depth_Keller msg: 1730867938.11 epoch sec; seqNo:547; depth 37.38 2024-11-06T04:38:58Z- handleDVL() - DVL msg epochMillisec: 1730867938.12 2024-11-06T04:38:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:38:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:38:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:38:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:38:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:38:58Z- handleDvl() - RDI_Pathfinder msg: 1730867938.12 sec; seqNo:181; #beams:0 2024-11-06T04:38:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:38:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.06 , -0.01 2024-11-06T04:38:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867938.13 epoch sec; seqNo:547; 4061598.84 north; 594316.85 east; 36.70 lat; -121.94 lon; 37.38 depth; utmZone: 10 2024-11-06T04:38:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867937.71 2024-11-06T04:38:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:58Z- waiting for 1730867941.71, time is 1730867938.13 2024-11-06T04:38:58Z- AHRS_M2 msg: 1730867938.50 epoch sec; seqNo:510 2024-11-06T04:38:58Z- AHRS_M2 msg: 0.13 phi; -0.18 theta; 5.48 psi 2024-11-06T04:38:58Z- handleDepth()- Depth_Keller msg: 1730867938.51 epoch sec; seqNo:548; depth 37.43 2024-11-06T04:38:58Z- handleDVL() - DVL msg epochMillisec: 1730867938.54 2024-11-06T04:38:58Z- handleDvl() - RDI_Pathfinder msg: 1730867938.54 sec; seqNo:182; #beams:4 2024-11-06T04:38:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.72 , 51.19 , 50.92 , 61.81 2024-11-06T04:38:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.05 , -0.01 2024-11-06T04:38:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867938.56 epoch sec; seqNo:548; 4061598.84 north; 594316.85 east; 36.70 lat; -121.94 lon; 37.43 depth; utmZone: 10 2024-11-06T04:38:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:38:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867937.71 2024-11-06T04:38:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:58Z- waiting for 1730867941.71, time is 1730867938.56 2024-11-06T04:38:58Z- AHRS_M2 msg: 1730867938.90 epoch sec; seqNo:511 2024-11-06T04:38:58Z- AHRS_M2 msg: 0.14 phi; -0.18 theta; 5.48 psi 2024-11-06T04:38:58Z- handleDepth()- Depth_Keller msg: 1730867938.92 epoch sec; seqNo:549; depth 37.52 2024-11-06T04:38:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867938.93 epoch sec; seqNo:549; 4061599.22 north; 594316.85 east; 36.70 lat; -121.94 lon; 37.52 depth; utmZone: 10 2024-11-06T04:38:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867937.71 2024-11-06T04:38:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:59Z- waiting for 1730867941.71, time is 1730867938.93 2024-11-06T04:38:59Z- handleDepth()- Depth_Keller msg: 1730867939.33 epoch sec; seqNo:550; depth 37.56 2024-11-06T04:38:59Z- handleDVL() - DVL msg epochMillisec: 1730867939.33 2024-11-06T04:38:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:38:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:38:59Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:38:59Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:38:59Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:38:59Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:38:59Z- handleDvl() - RDI_Pathfinder msg: 1730867939.33 sec; seqNo:183; #beams:0 2024-11-06T04:38:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:38:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.05 , -0.01 2024-11-06T04:38:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867939.36 epoch sec; seqNo:550; 4061599.58 north; 594315.62 east; 36.70 lat; -121.94 lon; 37.56 depth; utmZone: 10 2024-11-06T04:38:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867937.71 2024-11-06T04:38:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:59Z- waiting for 1730867941.71, time is 1730867939.36 2024-11-06T04:38:59Z- AHRS_M2 msg: 1730867939.71 epoch sec; seqNo:512 2024-11-06T04:38:59Z- AHRS_M2 msg: 0.14 phi; -0.19 theta; 5.49 psi 2024-11-06T04:38:59Z- handleDepth()- Depth_Keller msg: 1730867939.72 epoch sec; seqNo:551; depth 37.62 2024-11-06T04:38:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867939.73 epoch sec; seqNo:551; 4061599.58 north; 594315.62 east; 36.70 lat; -121.94 lon; 37.62 depth; utmZone: 10 2024-11-06T04:38:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:38:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867937.71 2024-11-06T04:38:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:38:59Z- waiting for 1730867941.71, time is 1730867939.73 2024-11-06T04:39:00Z- AHRS_M2 msg: 1730867940.12 epoch sec; seqNo:513 2024-11-06T04:39:00Z- AHRS_M2 msg: 0.14 phi; -0.19 theta; 5.49 psi 2024-11-06T04:39:00Z- handleDepth()- Depth_Keller msg: 1730867940.12 epoch sec; seqNo:552; depth 37.68 2024-11-06T04:39:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867940.15 epoch sec; seqNo:552; 4061599.96 north; 594315.62 east; 36.70 lat; -121.94 lon; 37.68 depth; utmZone: 10 2024-11-06T04:39:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867937.71 2024-11-06T04:39:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:00Z- waiting for 1730867941.71, time is 1730867940.15 2024-11-06T04:39:00Z- AHRS_M2 msg: 1730867940.53 epoch sec; seqNo:514 2024-11-06T04:39:00Z- AHRS_M2 msg: 0.13 phi; -0.19 theta; 5.49 psi 2024-11-06T04:39:00Z- handleDepth()- Depth_Keller msg: 1730867940.54 epoch sec; seqNo:553; depth 37.75 2024-11-06T04:39:00Z- handleDVL() - DVL msg epochMillisec: 1730867940.56 2024-11-06T04:39:00Z- handleDvl() - RDI_Pathfinder msg: 1730867940.56 sec; seqNo:184; #beams:4 2024-11-06T04:39:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.01 , 50.80 , 51.05 , 61.77 2024-11-06T04:39:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.08 , -0.00 2024-11-06T04:39:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867940.58 epoch sec; seqNo:553; 4061600.34 north; 594315.62 east; 36.70 lat; -121.94 lon; 37.75 depth; utmZone: 10 2024-11-06T04:39:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867937.71 2024-11-06T04:39:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:00Z- waiting for 1730867941.71, time is 1730867940.58 2024-11-06T04:39:00Z- AHRS_M2 msg: 1730867940.93 epoch sec; seqNo:515 2024-11-06T04:39:00Z- AHRS_M2 msg: 0.13 phi; -0.19 theta; 5.49 psi 2024-11-06T04:39:00Z- handleDepth()- Depth_Keller msg: 1730867940.93 epoch sec; seqNo:554; depth 37.83 2024-11-06T04:39:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867940.95 epoch sec; seqNo:554; 4061600.33 north; 594314.40 east; 36.70 lat; -121.94 lon; 37.83 depth; utmZone: 10 2024-11-06T04:39:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867937.71 2024-11-06T04:39:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:01Z- waiting for 1730867941.71, time is 1730867940.95 2024-11-06T04:39:01Z- AHRS_M2 msg: 1730867941.33 epoch sec; seqNo:516 2024-11-06T04:39:01Z- AHRS_M2 msg: 0.13 phi; -0.19 theta; 5.49 psi 2024-11-06T04:39:01Z- handleDepth()- Depth_Keller msg: 1730867941.34 epoch sec; seqNo:555; depth 37.88 2024-11-06T04:39:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867941.35 epoch sec; seqNo:555; 4061600.71 north; 594314.39 east; 36.70 lat; -121.94 lon; 37.88 depth; utmZone: 10 2024-11-06T04:39:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867937.71 2024-11-06T04:39:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:01Z- waiting for 1730867941.71, time is 1730867941.35 2024-11-06T04:39:01Z- AHRS_M2 msg: 1730867941.73 epoch sec; seqNo:517 2024-11-06T04:39:01Z- AHRS_M2 msg: 0.13 phi; -0.19 theta; 5.49 psi 2024-11-06T04:39:01Z- handleDepth()- Depth_Keller msg: 1730867941.74 epoch sec; seqNo:556; depth 37.94 2024-11-06T04:39:01Z- handleDVL() - DVL msg epochMillisec: 1730867941.74 2024-11-06T04:39:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:01Z- handleDvl() - RDI_Pathfinder msg: 1730867941.74 sec; seqNo:185; #beams:0 2024-11-06T04:39:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.08 , -0.00 2024-11-06T04:39:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867941.75 epoch sec; seqNo:556; 4061601.08 north; 594314.39 east; 36.70 lat; -121.94 lon; 37.94 depth; utmZone: 10 2024-11-06T04:39:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867937.71 2024-11-06T04:39:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:01Z- waiting for 1730867941.71, time is 1730867941.75 2024-11-06T04:39:01Z- LcmTrn::updateTrn() >>>> pose time 1730867941.75, meas time 1730867941.74 <<<< 2024-11-06T04:39:01Z- LcmTrn::updateTrn() equating pose 1730867941.74 and meas times 1730867941.74 2024-11-06T04:39:01Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867941.74 2024-11-06T04:39:01Z- TerrainNav::motionUpdate() - currEstimate = (4061601, 594314.39, 37.94) 2024-11-06T04:39:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:39:01Z- TerrainNav:: SubcloudNIS is 0.15, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:39:01Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:39:01Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:39:01Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:39:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:39:01Z- variances([0], [2], and psi_berg) = 0.074935 0.068041 0.000000 2024-11-06T04:39:01Z- publishEstimatesDecoder() - at 1730867941.74 sec 2024-11-06T04:39:01Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:39:01Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061545.20 594317.59 47.07 2024-11-06T04:39:01Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061545.05 594318.49 46.50 lat/lon: 36.69 -121.94 2024-11-06T04:39:01Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.07 0.07 1.84 0.00 2024-11-06T04:39:02Z- AHRS_M2 msg: 1730867942.14 epoch sec; seqNo:518 2024-11-06T04:39:02Z- AHRS_M2 msg: 0.13 phi; -0.19 theta; 5.49 psi 2024-11-06T04:39:02Z- handleDepth()- Depth_Keller msg: 1730867942.15 epoch sec; seqNo:557; depth 38.01 2024-11-06T04:39:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867942.17 epoch sec; seqNo:557; 4061601.08 north; 594314.39 east; 36.70 lat; -121.94 lon; 38.01 depth; utmZone: 10 2024-11-06T04:39:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867941.74 2024-11-06T04:39:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:02Z- waiting for 1730867945.74, time is 1730867942.17 2024-11-06T04:39:02Z- AHRS_M2 msg: 1730867942.54 epoch sec; seqNo:519 2024-11-06T04:39:02Z- AHRS_M2 msg: 0.13 phi; -0.19 theta; 5.49 psi 2024-11-06T04:39:02Z- handleDepth()- Depth_Keller msg: 1730867942.54 epoch sec; seqNo:558; depth 38.07 2024-11-06T04:39:02Z- handleDVL() - DVL msg epochMillisec: 1730867942.58 2024-11-06T04:39:02Z- handleDvl() - RDI_Pathfinder msg: 1730867942.58 sec; seqNo:186; #beams:4 2024-11-06T04:39:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.24 , 50.54 , 50.80 , 61.01 2024-11-06T04:39:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.09 , -0.02 2024-11-06T04:39:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867942.59 epoch sec; seqNo:558; 4061601.45 north; 594313.16 east; 36.70 lat; -121.94 lon; 38.07 depth; utmZone: 10 2024-11-06T04:39:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867941.74 2024-11-06T04:39:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:02Z- waiting for 1730867945.74, time is 1730867942.59 2024-11-06T04:39:02Z- AHRS_M2 msg: 1730867942.94 epoch sec; seqNo:520 2024-11-06T04:39:02Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.49 psi 2024-11-06T04:39:02Z- handleDepth()- Depth_Keller msg: 1730867942.95 epoch sec; seqNo:559; depth 38.14 2024-11-06T04:39:02Z- handleDVL() - DVL msg epochMillisec: 1730867942.95 2024-11-06T04:39:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:02Z- handleDvl() - RDI_Pathfinder msg: 1730867942.95 sec; seqNo:187; #beams:0 2024-11-06T04:39:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.09 , -0.02 2024-11-06T04:39:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867942.98 epoch sec; seqNo:559; 4061601.83 north; 594313.16 east; 36.70 lat; -121.94 lon; 38.14 depth; utmZone: 10 2024-11-06T04:39:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867941.74 2024-11-06T04:39:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:03Z- waiting for 1730867945.74, time is 1730867942.98 2024-11-06T04:39:03Z- AHRS_M2 msg: 1730867943.35 epoch sec; seqNo:521 2024-11-06T04:39:03Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.49 psi 2024-11-06T04:39:03Z- handleDepth()- Depth_Keller msg: 1730867943.35 epoch sec; seqNo:560; depth 38.20 2024-11-06T04:39:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867943.36 epoch sec; seqNo:560; 4061601.83 north; 594313.16 east; 36.70 lat; -121.94 lon; 38.20 depth; utmZone: 10 2024-11-06T04:39:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867941.74 2024-11-06T04:39:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:03Z- waiting for 1730867945.74, time is 1730867943.36 2024-11-06T04:39:03Z- AHRS_M2 msg: 1730867943.75 epoch sec; seqNo:522 2024-11-06T04:39:03Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.49 psi 2024-11-06T04:39:03Z- handleDepth()- Depth_Keller msg: 1730867943.76 epoch sec; seqNo:561; depth 38.26 2024-11-06T04:39:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867943.77 epoch sec; seqNo:561; 4061602.21 north; 594313.15 east; 36.70 lat; -121.94 lon; 38.26 depth; utmZone: 10 2024-11-06T04:39:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867941.74 2024-11-06T04:39:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:03Z- waiting for 1730867945.74, time is 1730867943.77 2024-11-06T04:39:04Z- AHRS_M2 msg: 1730867944.16 epoch sec; seqNo:523 2024-11-06T04:39:04Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.49 psi 2024-11-06T04:39:04Z- handleDepth()- Depth_Keller msg: 1730867944.16 epoch sec; seqNo:562; depth 38.33 2024-11-06T04:39:04Z- handleDVL() - DVL msg epochMillisec: 1730867944.17 2024-11-06T04:39:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:04Z- handleDvl() - RDI_Pathfinder msg: 1730867944.17 sec; seqNo:188; #beams:0 2024-11-06T04:39:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.09 , -0.02 2024-11-06T04:39:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867944.18 epoch sec; seqNo:562; 4061602.57 north; 594311.93 east; 36.70 lat; -121.94 lon; 38.33 depth; utmZone: 10 2024-11-06T04:39:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867941.74 2024-11-06T04:39:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:04Z- waiting for 1730867945.74, time is 1730867944.18 2024-11-06T04:39:04Z- AHRS_M2 msg: 1730867944.56 epoch sec; seqNo:524 2024-11-06T04:39:04Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.49 psi 2024-11-06T04:39:04Z- handleDepth()- Depth_Keller msg: 1730867944.57 epoch sec; seqNo:563; depth 38.38 2024-11-06T04:39:04Z- handleDVL() - DVL msg epochMillisec: 1730867944.59 2024-11-06T04:39:04Z- handleDvl() - RDI_Pathfinder msg: 1730867944.59 sec; seqNo:189; #beams:4 2024-11-06T04:39:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 59.22 , 50.28 , 50.03 , 60.49 2024-11-06T04:39:04Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.11 , -0.01 2024-11-06T04:39:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867944.60 epoch sec; seqNo:563; 4061602.57 north; 594311.93 east; 36.70 lat; -121.94 lon; 38.38 depth; utmZone: 10 2024-11-06T04:39:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867941.74 2024-11-06T04:39:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:04Z- waiting for 1730867945.74, time is 1730867944.60 2024-11-06T04:39:04Z- AHRS_M2 msg: 1730867944.96 epoch sec; seqNo:525 2024-11-06T04:39:04Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.49 psi 2024-11-06T04:39:04Z- handleDepth()- Depth_Keller msg: 1730867944.98 epoch sec; seqNo:564; depth 38.45 2024-11-06T04:39:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867944.99 epoch sec; seqNo:564; 4061602.95 north; 594311.93 east; 36.70 lat; -121.94 lon; 38.45 depth; utmZone: 10 2024-11-06T04:39:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867941.74 2024-11-06T04:39:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:05Z- waiting for 1730867945.74, time is 1730867944.99 2024-11-06T04:39:05Z- handleDepth()- Depth_Keller msg: 1730867945.39 epoch sec; seqNo:565; depth 38.52 2024-11-06T04:39:05Z- handleDVL() - DVL msg epochMillisec: 1730867945.40 2024-11-06T04:39:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:05Z- handleDvl() - RDI_Pathfinder msg: 1730867945.40 sec; seqNo:190; #beams:0 2024-11-06T04:39:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:05Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.11 , -0.01 2024-11-06T04:39:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867945.40 epoch sec; seqNo:565; 4061603.33 north; 594311.92 east; 36.70 lat; -121.94 lon; 38.52 depth; utmZone: 10 2024-11-06T04:39:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867941.74 2024-11-06T04:39:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:05Z- waiting for 1730867945.74, time is 1730867945.40 2024-11-06T04:39:05Z- AHRS_M2 msg: 1730867945.77 epoch sec; seqNo:526 2024-11-06T04:39:05Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.48 psi 2024-11-06T04:39:05Z- handleDepth()- Depth_Keller msg: 1730867945.78 epoch sec; seqNo:566; depth 38.59 2024-11-06T04:39:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867945.79 epoch sec; seqNo:566; 4061603.32 north; 594310.70 east; 36.70 lat; -121.94 lon; 38.59 depth; utmZone: 10 2024-11-06T04:39:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867941.74 2024-11-06T04:39:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:05Z- waiting for 1730867945.74, time is 1730867945.79 2024-11-06T04:39:05Z- LcmTrn::updateTrn() >>>> pose time 1730867945.79, meas time 1730867945.40 <<<< 2024-11-06T04:39:05Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867945.79 2024-11-06T04:39:05Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:39:05Z- TerrainNav::motionUpdate() - currEstimate = (4061603, 594310.70, 38.59) 2024-11-06T04:39:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:39:05Z- TerrainNav:: SubcloudNIS is 0.15, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:39:05Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:39:05Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:39:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:39:05Z- variances([0], [2], and psi_berg) = 0.077740 0.071570 0.000000 2024-11-06T04:39:05Z- publishEstimatesDecoder() - at 1730867945.79 sec 2024-11-06T04:39:05Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:39:05Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061547.39 594313.87 48.24 2024-11-06T04:39:05Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061547.28 594314.80 47.15 lat/lon: 36.69 -121.94 2024-11-06T04:39:05Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.08 0.07 2.08 0.00 2024-11-06T04:39:06Z- AHRS_M2 msg: 1730867946.18 epoch sec; seqNo:527 2024-11-06T04:39:06Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.48 psi 2024-11-06T04:39:06Z- handleDepth()- Depth_Keller msg: 1730867946.19 epoch sec; seqNo:567; depth 38.65 2024-11-06T04:39:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867946.21 epoch sec; seqNo:567; 4061603.70 north; 594310.70 east; 36.70 lat; -121.94 lon; 38.65 depth; utmZone: 10 2024-11-06T04:39:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867945.79 2024-11-06T04:39:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:06Z- waiting for 1730867949.79, time is 1730867946.21 2024-11-06T04:39:06Z- AHRS_M2 msg: 1730867946.58 epoch sec; seqNo:528 2024-11-06T04:39:06Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.48 psi 2024-11-06T04:39:06Z- handleDepth()- Depth_Keller msg: 1730867946.60 epoch sec; seqNo:568; depth 38.72 2024-11-06T04:39:06Z- handleDVL() - DVL msg epochMillisec: 1730867946.62 2024-11-06T04:39:06Z- handleDvl() - RDI_Pathfinder msg: 1730867946.62 sec; seqNo:191; #beams:4 2024-11-06T04:39:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 59.22 , 49.77 , 50.03 , 60.24 2024-11-06T04:39:06Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.11 , 0.03 2024-11-06T04:39:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867946.63 epoch sec; seqNo:568; 4061604.07 north; 594310.69 east; 36.70 lat; -121.94 lon; 38.72 depth; utmZone: 10 2024-11-06T04:39:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867945.79 2024-11-06T04:39:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:06Z- waiting for 1730867949.79, time is 1730867946.63 2024-11-06T04:39:06Z- AHRS_M2 msg: 1730867946.99 epoch sec; seqNo:529 2024-11-06T04:39:06Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.48 psi 2024-11-06T04:39:06Z- handleDepth()- Depth_Keller msg: 1730867946.99 epoch sec; seqNo:569; depth 38.78 2024-11-06T04:39:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867947.01 epoch sec; seqNo:569; 4061604.07 north; 594310.69 east; 36.70 lat; -121.94 lon; 38.78 depth; utmZone: 10 2024-11-06T04:39:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867945.79 2024-11-06T04:39:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:07Z- waiting for 1730867949.79, time is 1730867947.01 2024-11-06T04:39:07Z- AHRS_M2 msg: 1730867947.39 epoch sec; seqNo:530 2024-11-06T04:39:07Z- AHRS_M2 msg: 0.13 phi; -0.19 theta; 5.48 psi 2024-11-06T04:39:07Z- handleDepth()- Depth_Keller msg: 1730867947.39 epoch sec; seqNo:570; depth 38.85 2024-11-06T04:39:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867947.41 epoch sec; seqNo:570; 4061604.44 north; 594309.47 east; 36.70 lat; -121.94 lon; 38.85 depth; utmZone: 10 2024-11-06T04:39:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867945.79 2024-11-06T04:39:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:07Z- waiting for 1730867949.79, time is 1730867947.41 2024-11-06T04:39:07Z- AHRS_M2 msg: 1730867947.79 epoch sec; seqNo:531 2024-11-06T04:39:07Z- AHRS_M2 msg: 0.13 phi; -0.19 theta; 5.48 psi 2024-11-06T04:39:07Z- handleDepth()- Depth_Keller msg: 1730867947.80 epoch sec; seqNo:571; depth 38.93 2024-11-06T04:39:07Z- handleDVL() - DVL msg epochMillisec: 1730867947.80 2024-11-06T04:39:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:07Z- handleDvl() - RDI_Pathfinder msg: 1730867947.80 sec; seqNo:192; #beams:0 2024-11-06T04:39:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:07Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.11 , 0.03 2024-11-06T04:39:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867947.82 epoch sec; seqNo:571; 4061604.44 north; 594309.47 east; 36.70 lat; -121.94 lon; 38.93 depth; utmZone: 10 2024-11-06T04:39:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867945.79 2024-11-06T04:39:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:07Z- waiting for 1730867949.79, time is 1730867947.82 2024-11-06T04:39:08Z- AHRS_M2 msg: 1730867948.19 epoch sec; seqNo:532 2024-11-06T04:39:08Z- AHRS_M2 msg: 0.13 phi; -0.19 theta; 5.48 psi 2024-11-06T04:39:08Z- handleDepth()- Depth_Keller msg: 1730867948.20 epoch sec; seqNo:572; depth 39.01 2024-11-06T04:39:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867948.22 epoch sec; seqNo:572; 4061604.82 north; 594309.46 east; 36.70 lat; -121.94 lon; 39.01 depth; utmZone: 10 2024-11-06T04:39:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867945.79 2024-11-06T04:39:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:08Z- waiting for 1730867949.79, time is 1730867948.22 2024-11-06T04:39:08Z- AHRS_M2 msg: 1730867948.60 epoch sec; seqNo:533 2024-11-06T04:39:08Z- AHRS_M2 msg: 0.13 phi; -0.19 theta; 5.48 psi 2024-11-06T04:39:08Z- handleDepth()- Depth_Keller msg: 1730867948.60 epoch sec; seqNo:573; depth 39.05 2024-11-06T04:39:08Z- handleDVL() - DVL msg epochMillisec: 1730867948.62 2024-11-06T04:39:08Z- handleDvl() - RDI_Pathfinder msg: 1730867948.62 sec; seqNo:193; #beams:4 2024-11-06T04:39:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 59.21 , 49.77 , 49.77 , 60.23 2024-11-06T04:39:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.07 , 0.08 2024-11-06T04:39:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867948.65 epoch sec; seqNo:573; 4061605.20 north; 594309.46 east; 36.70 lat; -121.94 lon; 39.05 depth; utmZone: 10 2024-11-06T04:39:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867945.79 2024-11-06T04:39:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:08Z- waiting for 1730867949.79, time is 1730867948.65 2024-11-06T04:39:09Z- AHRS_M2 msg: 1730867949.01 epoch sec; seqNo:534 2024-11-06T04:39:09Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.48 psi 2024-11-06T04:39:09Z- handleDepth()- Depth_Keller msg: 1730867949.01 epoch sec; seqNo:574; depth 39.11 2024-11-06T04:39:09Z- handleDVL() - DVL msg epochMillisec: 1730867949.01 2024-11-06T04:39:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:09Z- handleDvl() - RDI_Pathfinder msg: 1730867949.01 sec; seqNo:194; #beams:0 2024-11-06T04:39:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.07 , 0.08 2024-11-06T04:39:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867949.03 epoch sec; seqNo:574; 4061605.18 north; 594308.24 east; 36.70 lat; -121.94 lon; 39.11 depth; utmZone: 10 2024-11-06T04:39:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867945.79 2024-11-06T04:39:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:09Z- waiting for 1730867949.79, time is 1730867949.03 2024-11-06T04:39:09Z- AHRS_M2 msg: 1730867949.41 epoch sec; seqNo:535 2024-11-06T04:39:09Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.48 psi 2024-11-06T04:39:09Z- handleDepth()- Depth_Keller msg: 1730867949.41 epoch sec; seqNo:575; depth 39.18 2024-11-06T04:39:09Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:39:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867945.79 2024-11-06T04:39:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:09Z- waiting for 1730867949.79, time is 1730867949.41 2024-11-06T04:39:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867949.57 epoch sec; seqNo:575; 4061605.56 north; 594308.24 east; 36.70 lat; -121.94 lon; 39.18 depth; utmZone: 10 2024-11-06T04:39:09Z- 1 msgs in 200 ms and 2 handle calls 2024-11-06T04:39:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867945.79 2024-11-06T04:39:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:09Z- waiting for 1730867949.79, time is 1730867949.57 2024-11-06T04:39:09Z- AHRS_M2 msg: 1730867949.81 epoch sec; seqNo:536 2024-11-06T04:39:09Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.48 psi 2024-11-06T04:39:09Z- handleDepth()- Depth_Keller msg: 1730867949.82 epoch sec; seqNo:576; depth 39.28 2024-11-06T04:39:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867949.83 epoch sec; seqNo:576; 4061605.94 north; 594308.23 east; 36.70 lat; -121.94 lon; 39.28 depth; utmZone: 10 2024-11-06T04:39:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867945.79 2024-11-06T04:39:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:09Z- waiting for 1730867949.79, time is 1730867949.83 2024-11-06T04:39:09Z- LcmTrn::updateTrn() >>>> pose time 1730867949.83, meas time 1730867949.01 <<<< 2024-11-06T04:39:09Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867949.83 2024-11-06T04:39:09Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:39:09Z- TerrainNav::motionUpdate() - currEstimate = (4061606, 594308.23, 39.28) 2024-11-06T04:39:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:39:09Z- TerrainNav:: SubcloudNIS is 0.15, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:39:09Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:39:09Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:39:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:39:09Z- variances([0], [2], and psi_berg) = 0.079581 0.073792 0.000000 2024-11-06T04:39:09Z- publishEstimatesDecoder() - at 1730867949.83 sec 2024-11-06T04:39:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:39:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061550.04 594311.36 48.93 2024-11-06T04:39:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061549.91 594312.33 47.85 lat/lon: 36.69 -121.94 2024-11-06T04:39:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.08 0.07 2.33 0.00 2024-11-06T04:39:10Z- AHRS_M2 msg: 1730867950.25 epoch sec; seqNo:537 2024-11-06T04:39:10Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.48 psi 2024-11-06T04:39:10Z- handleDepth()- Depth_Keller msg: 1730867950.26 epoch sec; seqNo:577; depth 39.31 2024-11-06T04:39:10Z- handleDVL() - DVL msg epochMillisec: 1730867950.27 2024-11-06T04:39:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:10Z- handleDvl() - RDI_Pathfinder msg: 1730867950.27 sec; seqNo:195; #beams:0 2024-11-06T04:39:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.07 , 0.08 2024-11-06T04:39:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867950.28 epoch sec; seqNo:577; 4061605.94 north; 594308.23 east; 36.70 lat; -121.94 lon; 39.31 depth; utmZone: 10 2024-11-06T04:39:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867949.83 2024-11-06T04:39:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:10Z- waiting for 1730867953.83, time is 1730867950.28 2024-11-06T04:39:10Z- AHRS_M2 msg: 1730867950.62 epoch sec; seqNo:538 2024-11-06T04:39:10Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.49 psi 2024-11-06T04:39:10Z- handleDepth()- Depth_Keller msg: 1730867950.63 epoch sec; seqNo:578; depth 39.37 2024-11-06T04:39:10Z- handleDVL() - DVL msg epochMillisec: 1730867950.66 2024-11-06T04:39:10Z- handleDvl() - RDI_Pathfinder msg: 1730867950.66 sec; seqNo:196; #beams:4 2024-11-06T04:39:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 58.70 , 50.02 , 49.77 , 60.23 2024-11-06T04:39:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.04 2024-11-06T04:39:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867950.67 epoch sec; seqNo:578; 4061606.31 north; 594307.01 east; 36.70 lat; -121.94 lon; 39.37 depth; utmZone: 10 2024-11-06T04:39:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867949.83 2024-11-06T04:39:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:10Z- waiting for 1730867953.83, time is 1730867950.67 2024-11-06T04:39:11Z- AHRS_M2 msg: 1730867951.03 epoch sec; seqNo:539 2024-11-06T04:39:11Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.49 psi 2024-11-06T04:39:11Z- handleDepth()- Depth_Keller msg: 1730867951.03 epoch sec; seqNo:579; depth 39.44 2024-11-06T04:39:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867951.04 epoch sec; seqNo:579; 4061606.69 north; 594307.00 east; 36.70 lat; -121.94 lon; 39.44 depth; utmZone: 10 2024-11-06T04:39:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867949.83 2024-11-06T04:39:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:11Z- waiting for 1730867953.83, time is 1730867951.04 2024-11-06T04:39:11Z- AHRS_M2 msg: 1730867951.43 epoch sec; seqNo:540 2024-11-06T04:39:11Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.49 psi 2024-11-06T04:39:11Z- handleDepth()- Depth_Keller msg: 1730867951.43 epoch sec; seqNo:580; depth 39.48 2024-11-06T04:39:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867951.44 epoch sec; seqNo:580; 4061606.69 north; 594307.00 east; 36.70 lat; -121.94 lon; 39.48 depth; utmZone: 10 2024-11-06T04:39:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867949.83 2024-11-06T04:39:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:11Z- waiting for 1730867953.83, time is 1730867951.44 2024-11-06T04:39:11Z- AHRS_M2 msg: 1730867951.84 epoch sec; seqNo:541 2024-11-06T04:39:11Z- AHRS_M2 msg: 0.11 phi; -0.19 theta; 5.49 psi 2024-11-06T04:39:11Z- handleDepth()- Depth_Keller msg: 1730867951.85 epoch sec; seqNo:581; depth 39.56 2024-11-06T04:39:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867951.86 epoch sec; seqNo:581; 4061607.07 north; 594307.00 east; 36.70 lat; -121.94 lon; 39.56 depth; utmZone: 10 2024-11-06T04:39:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867949.83 2024-11-06T04:39:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:11Z- waiting for 1730867953.83, time is 1730867951.86 2024-11-06T04:39:12Z- AHRS_M2 msg: 1730867952.24 epoch sec; seqNo:542 2024-11-06T04:39:12Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.49 psi 2024-11-06T04:39:12Z- handleDepth()- Depth_Keller msg: 1730867952.25 epoch sec; seqNo:582; depth 39.61 2024-11-06T04:39:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867952.27 epoch sec; seqNo:582; 4061607.07 north; 594307.00 east; 36.70 lat; -121.94 lon; 39.61 depth; utmZone: 10 2024-11-06T04:39:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867949.83 2024-11-06T04:39:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:12Z- waiting for 1730867953.83, time is 1730867952.27 2024-11-06T04:39:12Z- handleDepth()- Depth_Keller msg: 1730867952.66 epoch sec; seqNo:583; depth 39.66 2024-11-06T04:39:12Z- handleDVL() - DVL msg epochMillisec: 1730867952.69 2024-11-06T04:39:12Z- handleDvl() - RDI_Pathfinder msg: 1730867952.69 sec; seqNo:197; #beams:4 2024-11-06T04:39:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 58.19 , 50.02 , 49.26 , 59.72 2024-11-06T04:39:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.00 , -0.04 2024-11-06T04:39:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867952.70 epoch sec; seqNo:583; 4061607.43 north; 594305.77 east; 36.70 lat; -121.94 lon; 39.66 depth; utmZone: 10 2024-11-06T04:39:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867949.83 2024-11-06T04:39:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:12Z- waiting for 1730867953.83, time is 1730867952.70 2024-11-06T04:39:13Z- AHRS_M2 msg: 1730867953.05 epoch sec; seqNo:543 2024-11-06T04:39:13Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.49 psi 2024-11-06T04:39:13Z- handleDepth()- Depth_Keller msg: 1730867953.06 epoch sec; seqNo:584; depth 39.72 2024-11-06T04:39:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867953.08 epoch sec; seqNo:584; 4061607.81 north; 594305.77 east; 36.70 lat; -121.94 lon; 39.72 depth; utmZone: 10 2024-11-06T04:39:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867949.83 2024-11-06T04:39:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:13Z- waiting for 1730867953.83, time is 1730867953.08 2024-11-06T04:39:13Z- AHRS_M2 msg: 1730867953.45 epoch sec; seqNo:544 2024-11-06T04:39:13Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.49 psi 2024-11-06T04:39:13Z- handleDepth()- Depth_Keller msg: 1730867953.45 epoch sec; seqNo:585; depth 39.79 2024-11-06T04:39:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867953.48 epoch sec; seqNo:585; 4061608.19 north; 594305.77 east; 36.70 lat; -121.94 lon; 39.79 depth; utmZone: 10 2024-11-06T04:39:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867949.83 2024-11-06T04:39:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:13Z- waiting for 1730867953.83, time is 1730867953.48 2024-11-06T04:39:13Z- AHRS_M2 msg: 1730867953.85 epoch sec; seqNo:545 2024-11-06T04:39:13Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.49 psi 2024-11-06T04:39:13Z- handleDepth()- Depth_Keller msg: 1730867953.86 epoch sec; seqNo:586; depth 39.84 2024-11-06T04:39:13Z- handleDVL() - DVL msg epochMillisec: 1730867953.86 2024-11-06T04:39:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:13Z- handleDvl() - RDI_Pathfinder msg: 1730867953.86 sec; seqNo:198; #beams:0 2024-11-06T04:39:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.00 , -0.04 2024-11-06T04:39:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867953.87 epoch sec; seqNo:586; 4061608.19 north; 594305.77 east; 36.70 lat; -121.94 lon; 39.84 depth; utmZone: 10 2024-11-06T04:39:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867949.83 2024-11-06T04:39:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:14Z- waiting for 1730867953.83, time is 1730867953.87 2024-11-06T04:39:14Z- LcmTrn::updateTrn() >>>> pose time 1730867953.87, meas time 1730867953.86 <<<< 2024-11-06T04:39:14Z- LcmTrn::updateTrn() equating pose 1730867953.86 and meas times 1730867953.86 2024-11-06T04:39:14Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867953.86 2024-11-06T04:39:14Z- TerrainNav::motionUpdate() - currEstimate = (4061608, 594305.77, 39.84) 2024-11-06T04:39:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:39:14Z- TerrainNav:: SubcloudNIS is 0.15, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:39:14Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:39:14Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:39:14Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:39:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:39:14Z- variances([0], [2], and psi_berg) = 0.081619 0.076097 0.000000 2024-11-06T04:39:14Z- publishEstimatesDecoder() - at 1730867953.86 sec 2024-11-06T04:39:14Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:39:14Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061552.23 594308.90 49.31 2024-11-06T04:39:14Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061552.15 594309.87 48.41 lat/lon: 36.69 -121.94 2024-11-06T04:39:14Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.08 0.08 2.62 0.00 2024-11-06T04:39:14Z- AHRS_M2 msg: 1730867954.27 epoch sec; seqNo:546 2024-11-06T04:39:14Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.49 psi 2024-11-06T04:39:14Z- handleDepth()- Depth_Keller msg: 1730867954.28 epoch sec; seqNo:587; depth 39.90 2024-11-06T04:39:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867954.30 epoch sec; seqNo:587; 4061608.55 north; 594304.54 east; 36.70 lat; -121.94 lon; 39.90 depth; utmZone: 10 2024-11-06T04:39:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867953.86 2024-11-06T04:39:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:14Z- waiting for 1730867957.86, time is 1730867954.30 2024-11-06T04:39:14Z- handleDepth()- Depth_Keller msg: 1730867954.68 epoch sec; seqNo:588; depth 39.96 2024-11-06T04:39:14Z- handleDVL() - DVL msg epochMillisec: 1730867954.71 2024-11-06T04:39:14Z- handleDvl() - RDI_Pathfinder msg: 1730867954.71 sec; seqNo:199; #beams:4 2024-11-06T04:39:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 57.67 , 49.76 , 49.00 , 58.95 2024-11-06T04:39:14Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.04 , -0.05 2024-11-06T04:39:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867954.72 epoch sec; seqNo:588; 4061608.93 north; 594304.54 east; 36.70 lat; -121.94 lon; 39.96 depth; utmZone: 10 2024-11-06T04:39:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867953.86 2024-11-06T04:39:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:14Z- waiting for 1730867957.86, time is 1730867954.72 2024-11-06T04:39:15Z- AHRS_M2 msg: 1730867955.07 epoch sec; seqNo:547 2024-11-06T04:39:15Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.49 psi 2024-11-06T04:39:15Z- handleDepth()- Depth_Keller msg: 1730867955.07 epoch sec; seqNo:589; depth 40.03 2024-11-06T04:39:15Z- handleDVL() - DVL msg epochMillisec: 1730867955.08 2024-11-06T04:39:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:15Z- handleDvl() - RDI_Pathfinder msg: 1730867955.08 sec; seqNo:200; #beams:0 2024-11-06T04:39:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:15Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.04 , -0.05 2024-11-06T04:39:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867955.09 epoch sec; seqNo:589; 4061608.93 north; 594304.54 east; 36.70 lat; -121.94 lon; 40.03 depth; utmZone: 10 2024-11-06T04:39:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867953.86 2024-11-06T04:39:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:15Z- waiting for 1730867957.86, time is 1730867955.09 2024-11-06T04:39:15Z- AHRS_M2 msg: 1730867955.47 epoch sec; seqNo:548 2024-11-06T04:39:15Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.49 psi 2024-11-06T04:39:15Z- handleDepth()- Depth_Keller msg: 1730867955.49 epoch sec; seqNo:590; depth 40.08 2024-11-06T04:39:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867955.50 epoch sec; seqNo:590; 4061609.31 north; 594304.53 east; 36.70 lat; -121.94 lon; 40.08 depth; utmZone: 10 2024-11-06T04:39:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867953.86 2024-11-06T04:39:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:15Z- waiting for 1730867957.86, time is 1730867955.50 2024-11-06T04:39:15Z- handleDepth()- Depth_Keller msg: 1730867955.89 epoch sec; seqNo:591; depth 40.13 2024-11-06T04:39:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867955.91 epoch sec; seqNo:591; 4061609.68 north; 594303.31 east; 36.70 lat; -121.94 lon; 40.13 depth; utmZone: 10 2024-11-06T04:39:16Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:39:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867953.86 2024-11-06T04:39:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:16Z- waiting for 1730867957.86, time is 1730867955.91 2024-11-06T04:39:16Z- AHRS_M2 msg: 1730867956.28 epoch sec; seqNo:549 2024-11-06T04:39:16Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.48 psi 2024-11-06T04:39:16Z- handleDepth()- Depth_Keller msg: 1730867956.29 epoch sec; seqNo:592; depth 40.20 2024-11-06T04:39:16Z- handleDVL() - DVL msg epochMillisec: 1730867956.29 2024-11-06T04:39:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:16Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:16Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:16Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:16Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:16Z- handleDvl() - RDI_Pathfinder msg: 1730867956.29 sec; seqNo:201; #beams:0 2024-11-06T04:39:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:16Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.04 , -0.05 2024-11-06T04:39:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867956.31 epoch sec; seqNo:592; 4061610.06 north; 594303.30 east; 36.70 lat; -121.94 lon; 40.20 depth; utmZone: 10 2024-11-06T04:39:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867953.86 2024-11-06T04:39:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:16Z- waiting for 1730867957.86, time is 1730867956.31 2024-11-06T04:39:16Z- AHRS_M2 msg: 1730867956.68 epoch sec; seqNo:550 2024-11-06T04:39:16Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.48 psi 2024-11-06T04:39:16Z- handleDepth()- Depth_Keller msg: 1730867956.68 epoch sec; seqNo:593; depth 40.25 2024-11-06T04:39:16Z- handleDVL() - DVL msg epochMillisec: 1730867956.72 2024-11-06T04:39:16Z- handleDvl() - RDI_Pathfinder msg: 1730867956.72 sec; seqNo:202; #beams:4 2024-11-06T04:39:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 57.16 , 49.25 , 48.74 , 58.18 2024-11-06T04:39:16Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , -0.10 , -0.01 2024-11-06T04:39:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867956.73 epoch sec; seqNo:593; 4061610.06 north; 594303.30 east; 36.70 lat; -121.94 lon; 40.25 depth; utmZone: 10 2024-11-06T04:39:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867953.86 2024-11-06T04:39:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:16Z- waiting for 1730867957.86, time is 1730867956.73 2024-11-06T04:39:17Z- AHRS_M2 msg: 1730867957.08 epoch sec; seqNo:551 2024-11-06T04:39:17Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.48 psi 2024-11-06T04:39:17Z- handleDepth()- Depth_Keller msg: 1730867957.09 epoch sec; seqNo:594; depth 40.31 2024-11-06T04:39:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867957.11 epoch sec; seqNo:594; 4061610.43 north; 594303.30 east; 36.70 lat; -121.94 lon; 40.31 depth; utmZone: 10 2024-11-06T04:39:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867953.86 2024-11-06T04:39:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:17Z- waiting for 1730867957.86, time is 1730867957.11 2024-11-06T04:39:17Z- AHRS_M2 msg: 1730867957.49 epoch sec; seqNo:552 2024-11-06T04:39:17Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.47 psi 2024-11-06T04:39:17Z- handleDepth()- Depth_Keller msg: 1730867957.50 epoch sec; seqNo:595; depth 40.38 2024-11-06T04:39:17Z- handleDVL() - DVL msg epochMillisec: 1730867957.50 2024-11-06T04:39:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:17Z- handleDvl() - RDI_Pathfinder msg: 1730867957.50 sec; seqNo:203; #beams:0 2024-11-06T04:39:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:17Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , -0.10 , -0.01 2024-11-06T04:39:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867957.51 epoch sec; seqNo:595; 4061610.80 north; 594302.08 east; 36.70 lat; -121.94 lon; 40.38 depth; utmZone: 10 2024-11-06T04:39:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867953.86 2024-11-06T04:39:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:17Z- waiting for 1730867957.86, time is 1730867957.51 2024-11-06T04:39:17Z- AHRS_M2 msg: 1730867957.91 epoch sec; seqNo:553 2024-11-06T04:39:17Z- AHRS_M2 msg: 0.10 phi; -0.18 theta; 5.47 psi 2024-11-06T04:39:17Z- handleDepth()- Depth_Keller msg: 1730867957.92 epoch sec; seqNo:596; depth 40.44 2024-11-06T04:39:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867957.94 epoch sec; seqNo:596; 4061610.80 north; 594302.08 east; 36.70 lat; -121.94 lon; 40.44 depth; utmZone: 10 2024-11-06T04:39:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867953.86 2024-11-06T04:39:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:18Z- waiting for 1730867957.86, time is 1730867957.94 2024-11-06T04:39:18Z- LcmTrn::updateTrn() >>>> pose time 1730867957.94, meas time 1730867957.50 <<<< 2024-11-06T04:39:18Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867957.94 2024-11-06T04:39:18Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:39:18Z- TerrainNav::motionUpdate() - currEstimate = (4061611, 594302.08, 40.44) 2024-11-06T04:39:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:39:18Z- TerrainNav:: SubcloudNIS is 0.15, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:39:18Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:39:18Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:39:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:39:18Z- variances([0], [2], and psi_berg) = 0.084601 0.079341 0.000000 2024-11-06T04:39:18Z- publishEstimatesDecoder() - at 1730867957.94 sec 2024-11-06T04:39:18Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:39:18Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061554.78 594305.09 50.83 2024-11-06T04:39:18Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061554.77 594306.18 49.00 lat/lon: 36.69 -121.94 2024-11-06T04:39:18Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.08 0.08 2.87 0.00 2024-11-06T04:39:18Z- AHRS_M2 msg: 1730867958.29 epoch sec; seqNo:554 2024-11-06T04:39:18Z- AHRS_M2 msg: 0.10 phi; -0.18 theta; 5.47 psi 2024-11-06T04:39:18Z- handleDepth()- Depth_Keller msg: 1730867958.31 epoch sec; seqNo:597; depth 40.51 2024-11-06T04:39:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867958.34 epoch sec; seqNo:597; 4061611.18 north; 594302.07 east; 36.70 lat; -121.94 lon; 40.51 depth; utmZone: 10 2024-11-06T04:39:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867957.94 2024-11-06T04:39:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:18Z- waiting for 1730867961.94, time is 1730867958.34 2024-11-06T04:39:18Z- AHRS_M2 msg: 1730867958.70 epoch sec; seqNo:555 2024-11-06T04:39:18Z- AHRS_M2 msg: 0.10 phi; -0.17 theta; 5.46 psi 2024-11-06T04:39:18Z- handleDepth()- Depth_Keller msg: 1730867958.71 epoch sec; seqNo:598; depth 40.57 2024-11-06T04:39:18Z- handleDVL() - DVL msg epochMillisec: 1730867958.73 2024-11-06T04:39:18Z- handleDvl() - RDI_Pathfinder msg: 1730867958.73 sec; seqNo:204; #beams:4 2024-11-06T04:39:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 56.40 , 48.74 , 48.23 , 57.67 2024-11-06T04:39:18Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.12 , 0.03 2024-11-06T04:39:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867958.75 epoch sec; seqNo:598; 4061611.56 north; 594302.07 east; 36.70 lat; -121.94 lon; 40.57 depth; utmZone: 10 2024-11-06T04:39:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867957.94 2024-11-06T04:39:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:18Z- waiting for 1730867961.94, time is 1730867958.75 2024-11-06T04:39:19Z- AHRS_M2 msg: 1730867959.11 epoch sec; seqNo:556 2024-11-06T04:39:19Z- AHRS_M2 msg: 0.10 phi; -0.17 theta; 5.46 psi 2024-11-06T04:39:19Z- handleDepth()- Depth_Keller msg: 1730867959.11 epoch sec; seqNo:599; depth 40.63 2024-11-06T04:39:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867959.13 epoch sec; seqNo:599; 4061611.54 north; 594300.85 east; 36.70 lat; -121.94 lon; 40.63 depth; utmZone: 10 2024-11-06T04:39:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867957.94 2024-11-06T04:39:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:19Z- waiting for 1730867961.94, time is 1730867959.13 2024-11-06T04:39:19Z- AHRS_M2 msg: 1730867959.51 epoch sec; seqNo:557 2024-11-06T04:39:19Z- AHRS_M2 msg: 0.10 phi; -0.17 theta; 5.46 psi 2024-11-06T04:39:19Z- handleDepth()- Depth_Keller msg: 1730867959.51 epoch sec; seqNo:600; depth 40.69 2024-11-06T04:39:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867959.52 epoch sec; seqNo:600; 4061611.92 north; 594300.84 east; 36.70 lat; -121.94 lon; 40.69 depth; utmZone: 10 2024-11-06T04:39:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867957.94 2024-11-06T04:39:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:19Z- waiting for 1730867961.94, time is 1730867959.52 2024-11-06T04:39:19Z- AHRS_M2 msg: 1730867959.91 epoch sec; seqNo:558 2024-11-06T04:39:19Z- AHRS_M2 msg: 0.09 phi; -0.17 theta; 5.45 psi 2024-11-06T04:39:19Z- handleDepth()- Depth_Keller msg: 1730867959.92 epoch sec; seqNo:601; depth 40.75 2024-11-06T04:39:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867959.93 epoch sec; seqNo:601; 4061611.92 north; 594300.84 east; 36.70 lat; -121.94 lon; 40.75 depth; utmZone: 10 2024-11-06T04:39:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867957.94 2024-11-06T04:39:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:20Z- waiting for 1730867961.94, time is 1730867959.93 2024-11-06T04:39:20Z- AHRS_M2 msg: 1730867960.32 epoch sec; seqNo:559 2024-11-06T04:39:20Z- AHRS_M2 msg: 0.09 phi; -0.17 theta; 5.45 psi 2024-11-06T04:39:20Z- handleDepth()- Depth_Keller msg: 1730867960.34 epoch sec; seqNo:602; depth 40.82 2024-11-06T04:39:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867960.38 epoch sec; seqNo:602; 4061612.29 north; 594299.62 east; 36.70 lat; -121.94 lon; 40.82 depth; utmZone: 10 2024-11-06T04:39:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867957.94 2024-11-06T04:39:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:20Z- waiting for 1730867961.94, time is 1730867960.38 2024-11-06T04:39:20Z- AHRS_M2 msg: 1730867960.72 epoch sec; seqNo:560 2024-11-06T04:39:20Z- AHRS_M2 msg: 0.09 phi; -0.17 theta; 5.45 psi 2024-11-06T04:39:20Z- handleDepth()- Depth_Keller msg: 1730867960.72 epoch sec; seqNo:603; depth 40.89 2024-11-06T04:39:20Z- handleDVL() - DVL msg epochMillisec: 1730867960.75 2024-11-06T04:39:20Z- handleDvl() - RDI_Pathfinder msg: 1730867960.75 sec; seqNo:205; #beams:4 2024-11-06T04:39:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 55.88 , 48.99 , 48.48 , 57.16 2024-11-06T04:39:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.12 , 0.04 2024-11-06T04:39:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867960.76 epoch sec; seqNo:603; 4061612.67 north; 594299.61 east; 36.70 lat; -121.94 lon; 40.89 depth; utmZone: 10 2024-11-06T04:39:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867957.94 2024-11-06T04:39:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:20Z- waiting for 1730867961.94, time is 1730867960.76 2024-11-06T04:39:21Z- AHRS_M2 msg: 1730867961.12 epoch sec; seqNo:561 2024-11-06T04:39:21Z- AHRS_M2 msg: 0.09 phi; -0.17 theta; 5.45 psi 2024-11-06T04:39:21Z- handleDepth()- Depth_Keller msg: 1730867961.13 epoch sec; seqNo:604; depth 40.95 2024-11-06T04:39:21Z- handleDVL() - DVL msg epochMillisec: 1730867961.13 2024-11-06T04:39:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:21Z- handleDvl() - RDI_Pathfinder msg: 1730867961.13 sec; seqNo:206; #beams:0 2024-11-06T04:39:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.12 , 0.04 2024-11-06T04:39:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867961.15 epoch sec; seqNo:604; 4061612.67 north; 594299.61 east; 36.70 lat; -121.94 lon; 40.95 depth; utmZone: 10 2024-11-06T04:39:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867957.94 2024-11-06T04:39:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:21Z- waiting for 1730867961.94, time is 1730867961.15 2024-11-06T04:39:21Z- handleDepth()- Depth_Keller msg: 1730867961.55 epoch sec; seqNo:605; depth 41.02 2024-11-06T04:39:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867961.56 epoch sec; seqNo:605; 4061613.05 north; 594299.61 east; 36.70 lat; -121.94 lon; 41.02 depth; utmZone: 10 2024-11-06T04:39:21Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:39:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867957.94 2024-11-06T04:39:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:21Z- waiting for 1730867961.94, time is 1730867961.56 2024-11-06T04:39:21Z- AHRS_M2 msg: 1730867961.93 epoch sec; seqNo:562 2024-11-06T04:39:21Z- AHRS_M2 msg: 0.10 phi; -0.17 theta; 5.44 psi 2024-11-06T04:39:21Z- handleDepth()- Depth_Keller msg: 1730867961.94 epoch sec; seqNo:606; depth 41.09 2024-11-06T04:39:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867961.95 epoch sec; seqNo:606; 4061613.03 north; 594298.39 east; 36.70 lat; -121.94 lon; 41.09 depth; utmZone: 10 2024-11-06T04:39:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867957.94 2024-11-06T04:39:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:22Z- waiting for 1730867961.94, time is 1730867961.95 2024-11-06T04:39:22Z- LcmTrn::updateTrn() >>>> pose time 1730867961.95, meas time 1730867961.13 <<<< 2024-11-06T04:39:22Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867961.95 2024-11-06T04:39:22Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:39:22Z- TerrainNav::motionUpdate() - currEstimate = (4061613, 594298.39, 41.09) 2024-11-06T04:39:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:39:22Z- TerrainNav:: SubcloudNIS is 0.15, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:39:22Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:39:22Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:39:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:39:22Z- variances([0], [2], and psi_berg) = 0.087886 0.082275 0.000000 2024-11-06T04:39:22Z- publishEstimatesDecoder() - at 1730867961.95 sec 2024-11-06T04:39:22Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:39:22Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061557.05 594301.36 51.07 2024-11-06T04:39:22Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061557.00 594302.49 49.65 lat/lon: 36.69 -121.94 2024-11-06T04:39:22Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.09 0.08 3.08 0.00 2024-11-06T04:39:22Z- AHRS_M2 msg: 1730867962.34 epoch sec; seqNo:563 2024-11-06T04:39:22Z- AHRS_M2 msg: 0.10 phi; -0.17 theta; 5.44 psi 2024-11-06T04:39:22Z- handleDepth()- Depth_Keller msg: 1730867962.37 epoch sec; seqNo:607; depth 41.15 2024-11-06T04:39:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867962.41 epoch sec; seqNo:607; 4061613.41 north; 594298.38 east; 36.70 lat; -121.94 lon; 41.15 depth; utmZone: 10 2024-11-06T04:39:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867961.95 2024-11-06T04:39:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:22Z- waiting for 1730867965.95, time is 1730867962.41 2024-11-06T04:39:22Z- handleDepth()- Depth_Keller msg: 1730867962.76 epoch sec; seqNo:608; depth 41.23 2024-11-06T04:39:22Z- handleDVL() - DVL msg epochMillisec: 1730867962.78 2024-11-06T04:39:22Z- handleDvl() - RDI_Pathfinder msg: 1730867962.78 sec; seqNo:207; #beams:4 2024-11-06T04:39:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 55.88 , 48.99 , 48.23 , 57.16 2024-11-06T04:39:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.06 , 0.06 2024-11-06T04:39:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867962.80 epoch sec; seqNo:608; 4061613.79 north; 594298.38 east; 36.70 lat; -121.94 lon; 41.23 depth; utmZone: 10 2024-11-06T04:39:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867961.95 2024-11-06T04:39:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:22Z- waiting for 1730867965.95, time is 1730867962.80 2024-11-06T04:39:23Z- AHRS_M2 msg: 1730867963.14 epoch sec; seqNo:564 2024-11-06T04:39:23Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.44 psi 2024-11-06T04:39:23Z- handleDepth()- Depth_Keller msg: 1730867963.15 epoch sec; seqNo:609; depth 41.29 2024-11-06T04:39:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867963.16 epoch sec; seqNo:609; 4061613.79 north; 594298.38 east; 36.70 lat; -121.94 lon; 41.29 depth; utmZone: 10 2024-11-06T04:39:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867961.95 2024-11-06T04:39:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:23Z- waiting for 1730867965.95, time is 1730867963.16 2024-11-06T04:39:23Z- handleDepth()- Depth_Keller msg: 1730867963.58 epoch sec; seqNo:610; depth 41.34 2024-11-06T04:39:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867963.59 epoch sec; seqNo:610; 4061614.16 north; 594297.16 east; 36.70 lat; -121.94 lon; 41.34 depth; utmZone: 10 2024-11-06T04:39:23Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:39:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867961.95 2024-11-06T04:39:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:23Z- waiting for 1730867965.95, time is 1730867963.59 2024-11-06T04:39:23Z- handleDepth()- Depth_Keller msg: 1730867963.97 epoch sec; seqNo:611; depth 41.41 2024-11-06T04:39:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867963.98 epoch sec; seqNo:611; 4061614.16 north; 594297.16 east; 36.70 lat; -121.94 lon; 41.41 depth; utmZone: 10 2024-11-06T04:39:24Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:39:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867961.95 2024-11-06T04:39:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:24Z- waiting for 1730867965.95, time is 1730867963.98 2024-11-06T04:39:24Z- AHRS_M2 msg: 1730867964.41 epoch sec; seqNo:565 2024-11-06T04:39:24Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.43 psi 2024-11-06T04:39:24Z- handleDepth()- Depth_Keller msg: 1730867964.41 epoch sec; seqNo:612; depth 41.46 2024-11-06T04:39:24Z- handleDVL() - DVL msg epochMillisec: 1730867964.45 2024-11-06T04:39:24Z- handleDvl() - RDI_Pathfinder msg: 1730867964.45 sec; seqNo:208; #beams:4 2024-11-06T04:39:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 55.97 , 48.58 , 48.11 , 56.92 2024-11-06T04:39:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.06 2024-11-06T04:39:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867964.46 epoch sec; seqNo:612; 4061614.53 north; 594297.15 east; 36.70 lat; -121.94 lon; 41.46 depth; utmZone: 10 2024-11-06T04:39:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867961.95 2024-11-06T04:39:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:24Z- waiting for 1730867965.95, time is 1730867964.46 2024-11-06T04:39:24Z- AHRS_M2 msg: 1730867964.76 epoch sec; seqNo:566 2024-11-06T04:39:24Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.43 psi 2024-11-06T04:39:24Z- handleDepth()- Depth_Keller msg: 1730867964.77 epoch sec; seqNo:613; depth 41.53 2024-11-06T04:39:24Z- handleDVL() - DVL msg epochMillisec: 1730867964.77 2024-11-06T04:39:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:24Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:24Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:24Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:24Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:24Z- handleDvl() - RDI_Pathfinder msg: 1730867964.77 sec; seqNo:209; #beams:0 2024-11-06T04:39:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.06 2024-11-06T04:39:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867964.79 epoch sec; seqNo:613; 4061614.91 north; 594297.15 east; 36.70 lat; -121.94 lon; 41.53 depth; utmZone: 10 2024-11-06T04:39:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867961.95 2024-11-06T04:39:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:24Z- waiting for 1730867965.95, time is 1730867964.79 2024-11-06T04:39:25Z- AHRS_M2 msg: 1730867965.17 epoch sec; seqNo:567 2024-11-06T04:39:25Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.43 psi 2024-11-06T04:39:25Z- handleDepth()- Depth_Keller msg: 1730867965.17 epoch sec; seqNo:614; depth 41.57 2024-11-06T04:39:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867965.18 epoch sec; seqNo:614; 4061614.90 north; 594295.93 east; 36.70 lat; -121.94 lon; 41.57 depth; utmZone: 10 2024-11-06T04:39:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867961.95 2024-11-06T04:39:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:25Z- waiting for 1730867965.95, time is 1730867965.18 2024-11-06T04:39:25Z- AHRS_M2 msg: 1730867965.58 epoch sec; seqNo:568 2024-11-06T04:39:25Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.43 psi 2024-11-06T04:39:25Z- handleDepth()- Depth_Keller msg: 1730867965.59 epoch sec; seqNo:615; depth 41.63 2024-11-06T04:39:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867965.60 epoch sec; seqNo:615; 4061615.28 north; 594295.92 east; 36.70 lat; -121.94 lon; 41.63 depth; utmZone: 10 2024-11-06T04:39:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867961.95 2024-11-06T04:39:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:25Z- waiting for 1730867965.95, time is 1730867965.60 2024-11-06T04:39:25Z- AHRS_M2 msg: 1730867965.98 epoch sec; seqNo:569 2024-11-06T04:39:25Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.44 psi 2024-11-06T04:39:25Z- handleDepth()- Depth_Keller msg: 1730867965.98 epoch sec; seqNo:616; depth 41.69 2024-11-06T04:39:25Z- handleDVL() - DVL msg epochMillisec: 1730867965.98 2024-11-06T04:39:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:25Z- handleDvl() - RDI_Pathfinder msg: 1730867965.98 sec; seqNo:210; #beams:0 2024-11-06T04:39:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.06 2024-11-06T04:39:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867965.99 epoch sec; seqNo:616; 4061615.66 north; 594295.92 east; 36.70 lat; -121.94 lon; 41.69 depth; utmZone: 10 2024-11-06T04:39:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867961.95 2024-11-06T04:39:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:26Z- waiting for 1730867965.95, time is 1730867965.99 2024-11-06T04:39:26Z- LcmTrn::updateTrn() >>>> pose time 1730867965.99, meas time 1730867965.98 <<<< 2024-11-06T04:39:26Z- LcmTrn::updateTrn() equating pose 1730867965.98 and meas times 1730867965.98 2024-11-06T04:39:26Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867965.98 2024-11-06T04:39:26Z- TerrainNav::motionUpdate() - currEstimate = (4061616, 594295.92, 41.69) 2024-11-06T04:39:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:39:26Z- TerrainNav:: SubcloudNIS is 0.15, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:39:26Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:39:26Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:39:26Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:39:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:39:26Z- variances([0], [2], and psi_berg) = 0.089824 0.084381 0.000000 2024-11-06T04:39:26Z- publishEstimatesDecoder() - at 1730867965.98 sec 2024-11-06T04:39:26Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:39:26Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061559.66 594298.92 51.54 2024-11-06T04:39:26Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061559.62 594300.02 50.25 lat/lon: 36.70 -121.94 2024-11-06T04:39:26Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.09 0.08 3.32 0.00 2024-11-06T04:39:26Z- handleDepth()- Depth_Keller msg: 1730867966.43 epoch sec; seqNo:617; depth 41.73 2024-11-06T04:39:26Z- handleDVL() - DVL msg epochMillisec: 1730867966.44 2024-11-06T04:39:26Z- handleDvl() - RDI_Pathfinder msg: 1730867966.44 sec; seqNo:211; #beams:4 2024-11-06T04:39:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 55.97 , 48.35 , 48.11 , 56.68 2024-11-06T04:39:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.01 2024-11-06T04:39:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867966.46 epoch sec; seqNo:617; 4061615.66 north; 594295.92 east; 36.70 lat; -121.94 lon; 41.73 depth; utmZone: 10 2024-11-06T04:39:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867965.98 2024-11-06T04:39:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:26Z- waiting for 1730867969.98, time is 1730867966.46 2024-11-06T04:39:26Z- AHRS_M2 msg: 1730867966.78 epoch sec; seqNo:570 2024-11-06T04:39:26Z- AHRS_M2 msg: 0.12 phi; -0.17 theta; 5.44 psi 2024-11-06T04:39:26Z- handleDepth()- Depth_Keller msg: 1730867966.79 epoch sec; seqNo:618; depth 41.80 2024-11-06T04:39:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867966.81 epoch sec; seqNo:618; 4061616.04 north; 594295.92 east; 36.70 lat; -121.94 lon; 41.80 depth; utmZone: 10 2024-11-06T04:39:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867965.98 2024-11-06T04:39:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:26Z- waiting for 1730867969.98, time is 1730867966.81 2024-11-06T04:39:27Z- AHRS_M2 msg: 1730867967.19 epoch sec; seqNo:571 2024-11-06T04:39:27Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.44 psi 2024-11-06T04:39:27Z- handleDepth()- Depth_Keller msg: 1730867967.20 epoch sec; seqNo:619; depth 41.84 2024-11-06T04:39:27Z- handleDVL() - DVL msg epochMillisec: 1730867967.21 2024-11-06T04:39:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:27Z- handleDvl() - RDI_Pathfinder msg: 1730867967.21 sec; seqNo:212; #beams:0 2024-11-06T04:39:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.01 2024-11-06T04:39:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867967.21 epoch sec; seqNo:619; 4061616.40 north; 594294.69 east; 36.70 lat; -121.94 lon; 41.84 depth; utmZone: 10 2024-11-06T04:39:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867965.98 2024-11-06T04:39:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:27Z- waiting for 1730867969.98, time is 1730867967.21 2024-11-06T04:39:27Z- AHRS_M2 msg: 1730867967.60 epoch sec; seqNo:572 2024-11-06T04:39:27Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.44 psi 2024-11-06T04:39:27Z- handleDepth()- Depth_Keller msg: 1730867967.61 epoch sec; seqNo:620; depth 41.89 2024-11-06T04:39:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867967.63 epoch sec; seqNo:620; 4061616.40 north; 594294.69 east; 36.70 lat; -121.94 lon; 41.89 depth; utmZone: 10 2024-11-06T04:39:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867965.98 2024-11-06T04:39:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:27Z- waiting for 1730867969.98, time is 1730867967.63 2024-11-06T04:39:28Z- AHRS_M2 msg: 1730867968.00 epoch sec; seqNo:573 2024-11-06T04:39:28Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.44 psi 2024-11-06T04:39:28Z- handleDepth()- Depth_Keller msg: 1730867968.00 epoch sec; seqNo:621; depth 41.94 2024-11-06T04:39:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867968.01 epoch sec; seqNo:621; 4061616.78 north; 594294.69 east; 36.70 lat; -121.94 lon; 41.94 depth; utmZone: 10 2024-11-06T04:39:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867965.98 2024-11-06T04:39:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:28Z- waiting for 1730867969.98, time is 1730867968.01 2024-11-06T04:39:28Z- handleDepth()- Depth_Keller msg: 1730867968.44 epoch sec; seqNo:622; depth 41.99 2024-11-06T04:39:28Z- handleDVL() - DVL msg epochMillisec: 1730867968.46 2024-11-06T04:39:28Z- handleDvl() - RDI_Pathfinder msg: 1730867968.46 sec; seqNo:213; #beams:4 2024-11-06T04:39:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 55.48 , 48.10 , 47.63 , 56.67 2024-11-06T04:39:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.05 , -0.07 2024-11-06T04:39:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867968.48 epoch sec; seqNo:622; 4061617.16 north; 594294.68 east; 36.70 lat; -121.94 lon; 41.99 depth; utmZone: 10 2024-11-06T04:39:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867965.98 2024-11-06T04:39:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:28Z- waiting for 1730867969.98, time is 1730867968.48 2024-11-06T04:39:28Z- AHRS_M2 msg: 1730867968.80 epoch sec; seqNo:574 2024-11-06T04:39:28Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.43 psi 2024-11-06T04:39:28Z- handleDepth()- Depth_Keller msg: 1730867968.81 epoch sec; seqNo:623; depth 42.05 2024-11-06T04:39:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867968.85 epoch sec; seqNo:623; 4061617.15 north; 594293.46 east; 36.70 lat; -121.94 lon; 42.05 depth; utmZone: 10 2024-11-06T04:39:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867965.98 2024-11-06T04:39:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:28Z- waiting for 1730867969.98, time is 1730867968.85 2024-11-06T04:39:29Z- AHRS_M2 msg: 1730867969.21 epoch sec; seqNo:575 2024-11-06T04:39:29Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.43 psi 2024-11-06T04:39:29Z- handleDepth()- Depth_Keller msg: 1730867969.22 epoch sec; seqNo:624; depth 42.09 2024-11-06T04:39:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867969.23 epoch sec; seqNo:624; 4061617.52 north; 594293.46 east; 36.70 lat; -121.94 lon; 42.09 depth; utmZone: 10 2024-11-06T04:39:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867965.98 2024-11-06T04:39:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:29Z- waiting for 1730867969.98, time is 1730867969.23 2024-11-06T04:39:29Z- AHRS_M2 msg: 1730867969.61 epoch sec; seqNo:576 2024-11-06T04:39:29Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.43 psi 2024-11-06T04:39:29Z- handleDepth()- Depth_Keller msg: 1730867969.62 epoch sec; seqNo:625; depth 42.13 2024-11-06T04:39:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867969.64 epoch sec; seqNo:625; 4061617.52 north; 594293.46 east; 36.70 lat; -121.94 lon; 42.13 depth; utmZone: 10 2024-11-06T04:39:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867965.98 2024-11-06T04:39:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:29Z- waiting for 1730867969.98, time is 1730867969.64 2024-11-06T04:39:30Z- AHRS_M2 msg: 1730867970.02 epoch sec; seqNo:577 2024-11-06T04:39:30Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.43 psi 2024-11-06T04:39:30Z- handleDepth()- Depth_Keller msg: 1730867970.02 epoch sec; seqNo:626; depth 42.18 2024-11-06T04:39:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867970.03 epoch sec; seqNo:626; 4061617.90 north; 594293.45 east; 36.70 lat; -121.94 lon; 42.18 depth; utmZone: 10 2024-11-06T04:39:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867965.98 2024-11-06T04:39:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:30Z- waiting for 1730867969.98, time is 1730867970.03 2024-11-06T04:39:30Z- LcmTrn::updateTrn() >>>> pose time 1730867970.03, meas time 1730867968.46 <<<< 2024-11-06T04:39:30Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867970.03 2024-11-06T04:39:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867968.46 sec, ping # 0; waiting for INS data more recent than 1730867965.98... 2024-11-06T04:39:30Z- TerrainNav::motionUpdate() - currEstimate = (4061618, 594293.45, 42.18) 2024-11-06T04:39:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:39:30Z- TerrainNav:: SubcloudNIS is 0.15, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:39:30Z- TNavPF::Projecting Measurements 2024-11-06T04:39:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:39:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:39:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:39:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:39:30Z- TNavFilter::4 good beams. 2024-11-06T04:39:30Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:39:30Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:39:30Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:39:30Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:39:30Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:39:30Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:39:30Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:39:30Z- TerrainMapOctree: (vehN, vehE ) = (4061617.03, 594294.41). 2024-11-06T04:39:30Z- TerrainMapOctree: Min Distance = 140.19. 2024-11-06T04:39:30Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:39:30Z- Forcing Subcloud Comparison 2024-11-06T04:39:30Z- Weighting particles with subcloud comparison 2024-11-06T04:39:30Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:39:30Z- meanExpectedMeasDiff: 1.126891 delta_rms_squared: 5.662317 alpha: 0.996480 2024-11-06T04:39:30Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:39:30Z- meanExpectedMeasDiff: 1.462825 delta_rms_squared: 4.366490 alpha: 0.995441 2024-11-06T04:39:30Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:39:30Z- meanExpectedMeasDiff: 1.373731 delta_rms_squared: 4.096897 alpha: 0.995142 2024-11-06T04:39:30Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:39:30Z- meanExpectedMeasDiff: 1.551180 delta_rms_squared: 6.103682 alpha: 0.996734 2024-11-06T04:39:30Z- Subcloud NIS: 0.154398 2024-11-06T04:39:30Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:39:30Z- TNavPF:: sumWeights = 0.411370 2024-11-06T04:39:30Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:39:30Z- TNavPF::First Term of Map Covariance Matrix is 0.54 2024-11-06T04:39:30Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T04:39:30Z- TerrainNav::Resampling particle filter... 2024-11-06T04:39:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867968.46 sec. 2024-11-06T04:39:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:39:30Z- variances([0], [2], and psi_berg) = 0.092967 0.085178 0.000000 2024-11-06T04:39:30Z- publishEstimatesDecoder() - at 1730867970.03 sec 2024-11-06T04:39:30Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:39:30Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061561.60 594297.37 50.90 2024-11-06T04:39:30Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061561.87 594297.56 49.83 lat/lon: 36.70 -121.94 2024-11-06T04:39:30Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.09 0.09 1.07 0.00 2024-11-06T04:39:30Z- AHRS_M2 msg: 1730867970.42 epoch sec; seqNo:578 2024-11-06T04:39:30Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.43 psi 2024-11-06T04:39:30Z- handleDepth()- Depth_Keller msg: 1730867970.44 epoch sec; seqNo:627; depth 42.23 2024-11-06T04:39:30Z- handleDVL() - DVL msg epochMillisec: 1730867970.49 2024-11-06T04:39:30Z- handleDvl() - RDI_Pathfinder msg: 1730867970.49 sec; seqNo:214; #beams:4 2024-11-06T04:39:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 54.77 , 47.63 , 47.15 , 56.20 2024-11-06T04:39:30Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.09 , -0.06 2024-11-06T04:39:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867970.53 epoch sec; seqNo:627; 4061618.27 north; 594292.23 east; 36.70 lat; -121.94 lon; 42.23 depth; utmZone: 10 2024-11-06T04:39:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867970.03 2024-11-06T04:39:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:30Z- waiting for 1730867974.03, time is 1730867970.53 2024-11-06T04:39:30Z- AHRS_M2 msg: 1730867970.83 epoch sec; seqNo:579 2024-11-06T04:39:30Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.43 psi 2024-11-06T04:39:30Z- handleDepth()- Depth_Keller msg: 1730867970.83 epoch sec; seqNo:628; depth 42.29 2024-11-06T04:39:30Z- handleDVL() - DVL msg epochMillisec: 1730867970.84 2024-11-06T04:39:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:30Z- handleDvl() - RDI_Pathfinder msg: 1730867970.84 sec; seqNo:215; #beams:0 2024-11-06T04:39:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:30Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.09 , -0.06 2024-11-06T04:39:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867970.86 epoch sec; seqNo:628; 4061618.27 north; 594292.23 east; 36.70 lat; -121.94 lon; 42.29 depth; utmZone: 10 2024-11-06T04:39:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867970.03 2024-11-06T04:39:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:30Z- waiting for 1730867974.03, time is 1730867970.86 2024-11-06T04:39:31Z- AHRS_M2 msg: 1730867971.23 epoch sec; seqNo:580 2024-11-06T04:39:31Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.43 psi 2024-11-06T04:39:31Z- handleDepth()- Depth_Keller msg: 1730867971.23 epoch sec; seqNo:629; depth 42.34 2024-11-06T04:39:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867971.24 epoch sec; seqNo:629; 4061618.65 north; 594292.22 east; 36.70 lat; -121.94 lon; 42.34 depth; utmZone: 10 2024-11-06T04:39:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867970.03 2024-11-06T04:39:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:31Z- waiting for 1730867974.03, time is 1730867971.24 2024-11-06T04:39:31Z- AHRS_M2 msg: 1730867971.63 epoch sec; seqNo:581 2024-11-06T04:39:31Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.43 psi 2024-11-06T04:39:31Z- handleDepth()- Depth_Keller msg: 1730867971.64 epoch sec; seqNo:630; depth 42.39 2024-11-06T04:39:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867971.65 epoch sec; seqNo:630; 4061619.03 north; 594292.22 east; 36.70 lat; -121.94 lon; 42.39 depth; utmZone: 10 2024-11-06T04:39:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867970.03 2024-11-06T04:39:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:31Z- waiting for 1730867974.03, time is 1730867971.65 2024-11-06T04:39:32Z- AHRS_M2 msg: 1730867972.04 epoch sec; seqNo:582 2024-11-06T04:39:32Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.42 psi 2024-11-06T04:39:32Z- handleDepth()- Depth_Keller msg: 1730867972.05 epoch sec; seqNo:631; depth 42.45 2024-11-06T04:39:32Z- handleDVL() - DVL msg epochMillisec: 1730867972.05 2024-11-06T04:39:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:32Z- handleDvl() - RDI_Pathfinder msg: 1730867972.05 sec; seqNo:216; #beams:0 2024-11-06T04:39:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:32Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.09 , -0.06 2024-11-06T04:39:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867972.06 epoch sec; seqNo:631; 4061619.01 north; 594291.00 east; 36.70 lat; -121.94 lon; 42.45 depth; utmZone: 10 2024-11-06T04:39:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867970.03 2024-11-06T04:39:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:32Z- waiting for 1730867974.03, time is 1730867972.06 2024-11-06T04:39:32Z- AHRS_M2 msg: 1730867972.44 epoch sec; seqNo:583 2024-11-06T04:39:32Z- AHRS_M2 msg: 0.12 phi; -0.16 theta; 5.42 psi 2024-11-06T04:39:32Z- handleDepth()- Depth_Keller msg: 1730867972.45 epoch sec; seqNo:632; depth 42.50 2024-11-06T04:39:32Z- handleDVL() - DVL msg epochMillisec: 1730867972.48 2024-11-06T04:39:32Z- handleDvl() - RDI_Pathfinder msg: 1730867972.48 sec; seqNo:217; #beams:4 2024-11-06T04:39:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 55.01 , 47.39 , 46.91 , 55.48 2024-11-06T04:39:32Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , -0.12 , 0.04 2024-11-06T04:39:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867972.50 epoch sec; seqNo:632; 4061619.39 north; 594291.00 east; 36.70 lat; -121.94 lon; 42.50 depth; utmZone: 10 2024-11-06T04:39:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867970.03 2024-11-06T04:39:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:32Z- waiting for 1730867974.03, time is 1730867972.50 2024-11-06T04:39:32Z- AHRS_M2 msg: 1730867972.85 epoch sec; seqNo:584 2024-11-06T04:39:32Z- AHRS_M2 msg: 0.12 phi; -0.16 theta; 5.42 psi 2024-11-06T04:39:32Z- handleDepth()- Depth_Keller msg: 1730867972.85 epoch sec; seqNo:633; depth 42.57 2024-11-06T04:39:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867972.87 epoch sec; seqNo:633; 4061619.77 north; 594290.99 east; 36.70 lat; -121.94 lon; 42.57 depth; utmZone: 10 2024-11-06T04:39:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867970.03 2024-11-06T04:39:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:33Z- waiting for 1730867974.03, time is 1730867972.87 2024-11-06T04:39:33Z- handleDepth()- Depth_Keller msg: 1730867973.27 epoch sec; seqNo:634; depth 42.62 2024-11-06T04:39:33Z- handleDVL() - DVL msg epochMillisec: 1730867973.27 2024-11-06T04:39:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:33Z- handleDvl() - RDI_Pathfinder msg: 1730867973.27 sec; seqNo:218; #beams:0 2024-11-06T04:39:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:33Z- handleDvl() - RDI_Pathfinder msg: velocities 1.06 , -0.12 , 0.04 2024-11-06T04:39:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867973.29 epoch sec; seqNo:634; 4061619.76 north; 594289.77 east; 36.70 lat; -121.94 lon; 42.62 depth; utmZone: 10 2024-11-06T04:39:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867970.03 2024-11-06T04:39:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:33Z- waiting for 1730867974.03, time is 1730867973.29 2024-11-06T04:39:33Z- AHRS_M2 msg: 1730867973.65 epoch sec; seqNo:585 2024-11-06T04:39:33Z- AHRS_M2 msg: 0.12 phi; -0.16 theta; 5.42 psi 2024-11-06T04:39:33Z- handleDepth()- Depth_Keller msg: 1730867973.66 epoch sec; seqNo:635; depth 42.69 2024-11-06T04:39:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867973.69 epoch sec; seqNo:635; 4061620.14 north; 594289.77 east; 36.70 lat; -121.94 lon; 42.69 depth; utmZone: 10 2024-11-06T04:39:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867970.03 2024-11-06T04:39:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:33Z- waiting for 1730867974.03, time is 1730867973.69 2024-11-06T04:39:34Z- AHRS_M2 msg: 1730867974.06 epoch sec; seqNo:586 2024-11-06T04:39:34Z- AHRS_M2 msg: 0.12 phi; -0.16 theta; 5.41 psi 2024-11-06T04:39:34Z- handleDepth()- Depth_Keller msg: 1730867974.08 epoch sec; seqNo:636; depth 42.75 2024-11-06T04:39:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867974.10 epoch sec; seqNo:636; 4061620.14 north; 594289.77 east; 36.70 lat; -121.94 lon; 42.75 depth; utmZone: 10 2024-11-06T04:39:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867970.03 2024-11-06T04:39:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:34Z- waiting for 1730867974.03, time is 1730867974.10 2024-11-06T04:39:34Z- LcmTrn::updateTrn() >>>> pose time 1730867974.10, meas time 1730867973.27 <<<< 2024-11-06T04:39:34Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867974.10 2024-11-06T04:39:34Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:39:34Z- TerrainNav::motionUpdate() - currEstimate = (4061620, 594289.77, 42.75) 2024-11-06T04:39:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:39:34Z- TerrainNav:: SubcloudNIS is 0.15, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:39:34Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:39:34Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:39:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:39:34Z- variances([0], [2], and psi_berg) = 0.096337 0.087962 0.000000 2024-11-06T04:39:34Z- publishEstimatesDecoder() - at 1730867974.10 sec 2024-11-06T04:39:34Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:39:34Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061563.78 594293.62 51.09 2024-11-06T04:39:34Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061564.10 594293.88 50.41 lat/lon: 36.70 -121.94 2024-11-06T04:39:34Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.10 0.09 1.32 0.00 2024-11-06T04:39:34Z- AHRS_M2 msg: 1730867974.47 epoch sec; seqNo:587 2024-11-06T04:39:34Z- AHRS_M2 msg: 0.12 phi; -0.16 theta; 5.41 psi 2024-11-06T04:39:34Z- handleDepth()- Depth_Keller msg: 1730867974.47 epoch sec; seqNo:637; depth 42.81 2024-11-06T04:39:34Z- handleDVL() - DVL msg epochMillisec: 1730867974.51 2024-11-06T04:39:34Z- handleDvl() - RDI_Pathfinder msg: 1730867974.51 sec; seqNo:219; #beams:4 2024-11-06T04:39:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 54.29 , 46.91 , 46.91 , 54.77 2024-11-06T04:39:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.11 , 0.10 2024-11-06T04:39:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867974.52 epoch sec; seqNo:637; 4061620.52 north; 594289.76 east; 36.70 lat; -121.94 lon; 42.81 depth; utmZone: 10 2024-11-06T04:39:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867974.10 2024-11-06T04:39:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:34Z- waiting for 1730867978.10, time is 1730867974.52 2024-11-06T04:39:34Z- AHRS_M2 msg: 1730867974.87 epoch sec; seqNo:588 2024-11-06T04:39:34Z- AHRS_M2 msg: 0.11 phi; -0.16 theta; 5.41 psi 2024-11-06T04:39:34Z- handleDepth()- Depth_Keller msg: 1730867974.87 epoch sec; seqNo:638; depth 42.88 2024-11-06T04:39:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867974.89 epoch sec; seqNo:638; 4061620.50 north; 594288.54 east; 36.70 lat; -121.94 lon; 42.88 depth; utmZone: 10 2024-11-06T04:39:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867974.10 2024-11-06T04:39:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:35Z- waiting for 1730867978.10, time is 1730867974.89 2024-11-06T04:39:35Z- handleDepth()- Depth_Keller msg: 1730867975.30 epoch sec; seqNo:639; depth 42.92 2024-11-06T04:39:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867975.31 epoch sec; seqNo:639; 4061620.88 north; 594288.54 east; 36.70 lat; -121.94 lon; 42.92 depth; utmZone: 10 2024-11-06T04:39:35Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:39:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867974.10 2024-11-06T04:39:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:35Z- waiting for 1730867978.10, time is 1730867975.31 2024-11-06T04:39:35Z- AHRS_M2 msg: 1730867975.67 epoch sec; seqNo:589 2024-11-06T04:39:35Z- AHRS_M2 msg: 0.11 phi; -0.16 theta; 5.41 psi 2024-11-06T04:39:35Z- handleDepth()- Depth_Keller msg: 1730867975.69 epoch sec; seqNo:640; depth 43.00 2024-11-06T04:39:35Z- handleDVL() - DVL msg epochMillisec: 1730867975.70 2024-11-06T04:39:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:35Z- handleDvl() - RDI_Pathfinder msg: 1730867975.70 sec; seqNo:220; #beams:0 2024-11-06T04:39:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.11 , 0.10 2024-11-06T04:39:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867975.70 epoch sec; seqNo:640; 4061621.26 north; 594288.53 east; 36.70 lat; -121.94 lon; 43.00 depth; utmZone: 10 2024-11-06T04:39:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867974.10 2024-11-06T04:39:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:35Z- waiting for 1730867978.10, time is 1730867975.70 2024-11-06T04:39:36Z- AHRS_M2 msg: 1730867976.08 epoch sec; seqNo:590 2024-11-06T04:39:36Z- AHRS_M2 msg: 0.11 phi; -0.15 theta; 5.41 psi 2024-11-06T04:39:36Z- handleDepth()- Depth_Keller msg: 1730867976.08 epoch sec; seqNo:641; depth 43.03 2024-11-06T04:39:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867976.09 epoch sec; seqNo:641; 4061621.26 north; 594288.53 east; 36.70 lat; -121.94 lon; 43.03 depth; utmZone: 10 2024-11-06T04:39:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867974.10 2024-11-06T04:39:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:36Z- waiting for 1730867978.10, time is 1730867976.09 2024-11-06T04:39:36Z- AHRS_M2 msg: 1730867976.48 epoch sec; seqNo:591 2024-11-06T04:39:36Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.42 psi 2024-11-06T04:39:36Z- handleDepth()- Depth_Keller msg: 1730867976.49 epoch sec; seqNo:642; depth 43.08 2024-11-06T04:39:36Z- handleDVL() - DVL msg epochMillisec: 1730867976.51 2024-11-06T04:39:36Z- handleDvl() - RDI_Pathfinder msg: 1730867976.51 sec; seqNo:221; #beams:4 2024-11-06T04:39:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 54.05 , 46.91 , 46.91 , 54.53 2024-11-06T04:39:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , 0.06 2024-11-06T04:39:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867976.53 epoch sec; seqNo:642; 4061621.63 north; 594287.31 east; 36.70 lat; -121.94 lon; 43.08 depth; utmZone: 10 2024-11-06T04:39:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867974.10 2024-11-06T04:39:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:36Z- waiting for 1730867978.10, time is 1730867976.53 2024-11-06T04:39:36Z- AHRS_M2 msg: 1730867976.88 epoch sec; seqNo:592 2024-11-06T04:39:36Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.42 psi 2024-11-06T04:39:36Z- handleDepth()- Depth_Keller msg: 1730867976.89 epoch sec; seqNo:643; depth 43.14 2024-11-06T04:39:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867976.90 epoch sec; seqNo:643; 4061621.63 north; 594287.31 east; 36.70 lat; -121.94 lon; 43.14 depth; utmZone: 10 2024-11-06T04:39:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867974.10 2024-11-06T04:39:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:37Z- waiting for 1730867978.10, time is 1730867976.90 2024-11-06T04:39:37Z- AHRS_M2 msg: 1730867977.29 epoch sec; seqNo:593 2024-11-06T04:39:37Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.42 psi 2024-11-06T04:39:37Z- handleDepth()- Depth_Keller msg: 1730867977.29 epoch sec; seqNo:644; depth 43.21 2024-11-06T04:39:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867977.31 epoch sec; seqNo:644; 4061622.00 north; 594287.31 east; 36.70 lat; -121.94 lon; 43.21 depth; utmZone: 10 2024-11-06T04:39:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867974.10 2024-11-06T04:39:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:37Z- waiting for 1730867978.10, time is 1730867977.31 2024-11-06T04:39:37Z- AHRS_M2 msg: 1730867977.69 epoch sec; seqNo:594 2024-11-06T04:39:37Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.42 psi 2024-11-06T04:39:37Z- handleDepth()- Depth_Keller msg: 1730867977.70 epoch sec; seqNo:645; depth 43.24 2024-11-06T04:39:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867977.71 epoch sec; seqNo:645; 4061622.00 north; 594287.31 east; 36.70 lat; -121.94 lon; 43.24 depth; utmZone: 10 2024-11-06T04:39:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867974.10 2024-11-06T04:39:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:37Z- waiting for 1730867978.10, time is 1730867977.71 2024-11-06T04:39:38Z- AHRS_M2 msg: 1730867978.10 epoch sec; seqNo:595 2024-11-06T04:39:38Z- AHRS_M2 msg: 0.13 phi; -0.15 theta; 5.42 psi 2024-11-06T04:39:38Z- handleDepth()- Depth_Keller msg: 1730867978.10 epoch sec; seqNo:646; depth 43.30 2024-11-06T04:39:38Z- handleDVL() - DVL msg epochMillisec: 1730867978.10 2024-11-06T04:39:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:38Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:38Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:38Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:38Z- handleDvl() - RDI_Pathfinder msg: 1730867978.10 sec; seqNo:222; #beams:0 2024-11-06T04:39:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , 0.06 2024-11-06T04:39:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867978.11 epoch sec; seqNo:646; 4061622.37 north; 594286.08 east; 36.70 lat; -121.94 lon; 43.30 depth; utmZone: 10 2024-11-06T04:39:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867974.10 2024-11-06T04:39:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:38Z- waiting for 1730867978.10, time is 1730867978.11 2024-11-06T04:39:38Z- LcmTrn::updateTrn() >>>> pose time 1730867978.11, meas time 1730867978.10 <<<< 2024-11-06T04:39:38Z- LcmTrn::updateTrn() equating pose 1730867978.10 and meas times 1730867978.10 2024-11-06T04:39:38Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867978.10 2024-11-06T04:39:38Z- TerrainNav::motionUpdate() - currEstimate = (4061622, 594286.08, 43.30) 2024-11-06T04:39:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:39:38Z- TerrainNav:: SubcloudNIS is 0.15, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:39:38Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:39:38Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:39:38Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:39:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:39:38Z- variances([0], [2], and psi_berg) = 0.099350 0.091292 0.000000 2024-11-06T04:39:38Z- publishEstimatesDecoder() - at 1730867978.10 sec 2024-11-06T04:39:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:39:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061565.94 594289.94 51.47 2024-11-06T04:39:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061566.33 594290.19 50.97 lat/lon: 36.70 -121.94 2024-11-06T04:39:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.10 0.09 1.56 0.00 2024-11-06T04:39:38Z- AHRS_M2 msg: 1730867978.50 epoch sec; seqNo:596 2024-11-06T04:39:38Z- AHRS_M2 msg: 0.13 phi; -0.14 theta; 5.42 psi 2024-11-06T04:39:38Z- handleDepth()- Depth_Keller msg: 1730867978.51 epoch sec; seqNo:647; depth 43.35 2024-11-06T04:39:38Z- handleDVL() - DVL msg epochMillisec: 1730867978.53 2024-11-06T04:39:38Z- handleDvl() - RDI_Pathfinder msg: 1730867978.53 sec; seqNo:223; #beams:4 2024-11-06T04:39:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 54.05 , 46.67 , 46.91 , 54.53 2024-11-06T04:39:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , -0.01 2024-11-06T04:39:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867978.56 epoch sec; seqNo:647; 4061622.75 north; 594286.08 east; 36.70 lat; -121.94 lon; 43.35 depth; utmZone: 10 2024-11-06T04:39:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867978.10 2024-11-06T04:39:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:38Z- waiting for 1730867982.10, time is 1730867978.56 2024-11-06T04:39:38Z- AHRS_M2 msg: 1730867978.90 epoch sec; seqNo:597 2024-11-06T04:39:38Z- AHRS_M2 msg: 0.13 phi; -0.14 theta; 5.43 psi 2024-11-06T04:39:38Z- handleDepth()- Depth_Keller msg: 1730867978.92 epoch sec; seqNo:648; depth 43.39 2024-11-06T04:39:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867978.93 epoch sec; seqNo:648; 4061622.75 north; 594286.08 east; 36.70 lat; -121.94 lon; 43.39 depth; utmZone: 10 2024-11-06T04:39:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867978.10 2024-11-06T04:39:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:39Z- waiting for 1730867982.10, time is 1730867978.93 2024-11-06T04:39:39Z- AHRS_M2 msg: 1730867979.31 epoch sec; seqNo:598 2024-11-06T04:39:39Z- AHRS_M2 msg: 0.13 phi; -0.14 theta; 5.43 psi 2024-11-06T04:39:39Z- handleDepth()- Depth_Keller msg: 1730867979.32 epoch sec; seqNo:649; depth 43.46 2024-11-06T04:39:39Z- handleDVL() - DVL msg epochMillisec: 1730867979.32 2024-11-06T04:39:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:39Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:39Z- handleDvl() - RDI_Pathfinder msg: 1730867979.32 sec; seqNo:224; #beams:0 2024-11-06T04:39:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , -0.01 2024-11-06T04:39:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867979.34 epoch sec; seqNo:649; 4061623.13 north; 594286.07 east; 36.70 lat; -121.94 lon; 43.46 depth; utmZone: 10 2024-11-06T04:39:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867978.10 2024-11-06T04:39:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:39Z- waiting for 1730867982.10, time is 1730867979.34 2024-11-06T04:39:39Z- AHRS_M2 msg: 1730867979.71 epoch sec; seqNo:599 2024-11-06T04:39:39Z- AHRS_M2 msg: 0.13 phi; -0.14 theta; 5.43 psi 2024-11-06T04:39:39Z- handleDepth()- Depth_Keller msg: 1730867979.72 epoch sec; seqNo:650; depth 43.48 2024-11-06T04:39:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867979.73 epoch sec; seqNo:650; 4061623.11 north; 594284.85 east; 36.70 lat; -121.94 lon; 43.48 depth; utmZone: 10 2024-11-06T04:39:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867978.10 2024-11-06T04:39:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:39Z- waiting for 1730867982.10, time is 1730867979.73 2024-11-06T04:39:40Z- AHRS_M2 msg: 1730867980.12 epoch sec; seqNo:600 2024-11-06T04:39:40Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.43 psi 2024-11-06T04:39:40Z- handleDepth()- Depth_Keller msg: 1730867980.13 epoch sec; seqNo:651; depth 43.53 2024-11-06T04:39:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867980.14 epoch sec; seqNo:651; 4061623.49 north; 594284.85 east; 36.70 lat; -121.94 lon; 43.53 depth; utmZone: 10 2024-11-06T04:39:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867978.10 2024-11-06T04:39:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:40Z- waiting for 1730867982.10, time is 1730867980.14 2024-11-06T04:39:40Z- AHRS_M2 msg: 1730867980.52 epoch sec; seqNo:601 2024-11-06T04:39:40Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.43 psi 2024-11-06T04:39:40Z- handleDepth()- Depth_Keller msg: 1730867980.54 epoch sec; seqNo:652; depth 43.56 2024-11-06T04:39:40Z- handleDVL() - DVL msg epochMillisec: 1730867980.56 2024-11-06T04:39:40Z- handleDvl() - RDI_Pathfinder msg: 1730867980.56 sec; seqNo:225; #beams:4 2024-11-06T04:39:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 54.05 , 46.43 , 47.15 , 53.81 2024-11-06T04:39:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.00 , -0.02 2024-11-06T04:39:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867980.57 epoch sec; seqNo:652; 4061623.87 north; 594284.84 east; 36.70 lat; -121.94 lon; 43.56 depth; utmZone: 10 2024-11-06T04:39:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867978.10 2024-11-06T04:39:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:40Z- waiting for 1730867982.10, time is 1730867980.57 2024-11-06T04:39:40Z- AHRS_M2 msg: 1730867980.92 epoch sec; seqNo:602 2024-11-06T04:39:40Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.43 psi 2024-11-06T04:39:40Z- handleDepth()- Depth_Keller msg: 1730867980.93 epoch sec; seqNo:653; depth 43.60 2024-11-06T04:39:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867980.94 epoch sec; seqNo:653; 4061623.87 north; 594284.84 east; 36.70 lat; -121.94 lon; 43.60 depth; utmZone: 10 2024-11-06T04:39:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867978.10 2024-11-06T04:39:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:41Z- waiting for 1730867982.10, time is 1730867980.94 2024-11-06T04:39:41Z- AHRS_M2 msg: 1730867981.33 epoch sec; seqNo:603 2024-11-06T04:39:41Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.43 psi 2024-11-06T04:39:41Z- handleDepth()- Depth_Keller msg: 1730867981.33 epoch sec; seqNo:654; depth 43.65 2024-11-06T04:39:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867981.34 epoch sec; seqNo:654; 4061624.24 north; 594283.62 east; 36.70 lat; -121.94 lon; 43.65 depth; utmZone: 10 2024-11-06T04:39:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867978.10 2024-11-06T04:39:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:41Z- waiting for 1730867982.10, time is 1730867981.34 2024-11-06T04:39:41Z- AHRS_M2 msg: 1730867981.75 epoch sec; seqNo:604 2024-11-06T04:39:41Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.43 psi 2024-11-06T04:39:41Z- handleDepth()- Depth_Keller msg: 1730867981.75 epoch sec; seqNo:655; depth 43.69 2024-11-06T04:39:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867981.76 epoch sec; seqNo:655; 4061624.62 north; 594283.62 east; 36.70 lat; -121.94 lon; 43.69 depth; utmZone: 10 2024-11-06T04:39:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867978.10 2024-11-06T04:39:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:41Z- waiting for 1730867982.10, time is 1730867981.76 2024-11-06T04:39:42Z- AHRS_M2 msg: 1730867982.13 epoch sec; seqNo:605 2024-11-06T04:39:42Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.43 psi 2024-11-06T04:39:42Z- handleDepth()- Depth_Keller msg: 1730867982.15 epoch sec; seqNo:656; depth 43.73 2024-11-06T04:39:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867982.16 epoch sec; seqNo:656; 4061624.62 north; 594283.62 east; 36.70 lat; -121.94 lon; 43.73 depth; utmZone: 10 2024-11-06T04:39:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867978.10 2024-11-06T04:39:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:42Z- waiting for 1730867982.10, time is 1730867982.16 2024-11-06T04:39:42Z- LcmTrn::updateTrn() >>>> pose time 1730867982.16, meas time 1730867980.56 <<<< 2024-11-06T04:39:42Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867982.16 2024-11-06T04:39:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867980.56 sec, ping # 0; waiting for INS data more recent than 1730867978.10... 2024-11-06T04:39:42Z- TerrainNav::motionUpdate() - currEstimate = (4061625, 594283.62, 43.73) 2024-11-06T04:39:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:39:42Z- TerrainNav:: SubcloudNIS is 0.15, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:39:42Z- TNavPF::Projecting Measurements 2024-11-06T04:39:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:39:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:39:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:39:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:39:42Z- TNavFilter::4 good beams. 2024-11-06T04:39:42Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:39:42Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:39:42Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:39:42Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:39:42Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:39:42Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:39:42Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:39:42Z- TerrainMapOctree: (vehN, vehE ) = (4061623.73, 594284.59). 2024-11-06T04:39:42Z- TerrainMapOctree: Min Distance = 130.93. 2024-11-06T04:39:42Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:39:42Z- Forcing Subcloud Comparison 2024-11-06T04:39:42Z- Weighting particles with subcloud comparison 2024-11-06T04:39:42Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:39:42Z- meanExpectedMeasDiff: 0.082175 delta_rms_squared: 2.903001 alpha: 0.993158 2024-11-06T04:39:42Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:39:42Z- meanExpectedMeasDiff: 0.548317 delta_rms_squared: 1.384298 alpha: 0.985758 2024-11-06T04:39:42Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:39:42Z- meanExpectedMeasDiff: 0.901435 delta_rms_squared: 2.128816 alpha: 0.990693 2024-11-06T04:39:42Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:39:42Z- meanExpectedMeasDiff: 0.569270 delta_rms_squared: 4.376448 alpha: 0.995451 2024-11-06T04:39:42Z- Subcloud NIS: 0.132749 2024-11-06T04:39:42Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:39:42Z- TNavPF:: sumWeights = 0.544985 2024-11-06T04:39:42Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:39:42Z- TNavPF::First Term of Map Covariance Matrix is 0.58 2024-11-06T04:39:42Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T04:39:42Z- TerrainNav::Resampling particle filter... 2024-11-06T04:39:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867980.56 sec. 2024-11-06T04:39:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:39:42Z- variances([0], [2], and psi_berg) = 0.099643 0.092175 0.000000 2024-11-06T04:39:42Z- publishEstimatesDecoder() - at 1730867982.16 sec 2024-11-06T04:39:42Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:39:42Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061568.13 594287.52 52.66 2024-11-06T04:39:42Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061568.57 594287.72 51.11 lat/lon: 36.70 -121.94 2024-11-06T04:39:42Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.10 0.09 0.89 0.00 2024-11-06T04:39:42Z- handleDepth()- Depth_Keller msg: 1730867982.57 epoch sec; seqNo:657; depth 43.76 2024-11-06T04:39:42Z- handleDVL() - DVL msg epochMillisec: 1730867982.59 2024-11-06T04:39:42Z- handleDvl() - RDI_Pathfinder msg: 1730867982.59 sec; seqNo:226; #beams:4 2024-11-06T04:39:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 53.57 , 46.19 , 47.15 , 53.10 2024-11-06T04:39:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.04 , 0.00 2024-11-06T04:39:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867982.61 epoch sec; seqNo:657; 4061624.99 north; 594283.61 east; 36.70 lat; -121.94 lon; 43.76 depth; utmZone: 10 2024-11-06T04:39:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867982.16 2024-11-06T04:39:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:42Z- waiting for 1730867986.16, time is 1730867982.61 2024-11-06T04:39:42Z- AHRS_M2 msg: 1730867982.94 epoch sec; seqNo:606 2024-11-06T04:39:42Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.43 psi 2024-11-06T04:39:42Z- handleDepth()- Depth_Keller msg: 1730867982.95 epoch sec; seqNo:658; depth 43.80 2024-11-06T04:39:42Z- handleDVL() - DVL msg epochMillisec: 1730867982.95 2024-11-06T04:39:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:42Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:42Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:42Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:42Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:42Z- handleDvl() - RDI_Pathfinder msg: 1730867982.95 sec; seqNo:227; #beams:0 2024-11-06T04:39:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.04 , 0.00 2024-11-06T04:39:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867982.97 epoch sec; seqNo:658; 4061625.37 north; 594283.61 east; 36.70 lat; -121.94 lon; 43.80 depth; utmZone: 10 2024-11-06T04:39:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867982.16 2024-11-06T04:39:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:43Z- waiting for 1730867986.16, time is 1730867982.97 2024-11-06T04:39:43Z- AHRS_M2 msg: 1730867983.35 epoch sec; seqNo:607 2024-11-06T04:39:43Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.43 psi 2024-11-06T04:39:43Z- handleDepth()- Depth_Keller msg: 1730867983.36 epoch sec; seqNo:659; depth 43.83 2024-11-06T04:39:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867983.37 epoch sec; seqNo:659; 4061625.36 north; 594282.39 east; 36.70 lat; -121.94 lon; 43.83 depth; utmZone: 10 2024-11-06T04:39:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867982.16 2024-11-06T04:39:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:43Z- waiting for 1730867986.16, time is 1730867983.37 2024-11-06T04:39:43Z- AHRS_M2 msg: 1730867983.75 epoch sec; seqNo:608 2024-11-06T04:39:43Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 5.43 psi 2024-11-06T04:39:43Z- handleDepth()- Depth_Keller msg: 1730867983.76 epoch sec; seqNo:660; depth 43.86 2024-11-06T04:39:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867983.79 epoch sec; seqNo:660; 4061625.74 north; 594282.38 east; 36.70 lat; -121.94 lon; 43.86 depth; utmZone: 10 2024-11-06T04:39:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867982.16 2024-11-06T04:39:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:43Z- waiting for 1730867986.16, time is 1730867983.79 2024-11-06T04:39:44Z- AHRS_M2 msg: 1730867984.15 epoch sec; seqNo:609 2024-11-06T04:39:44Z- AHRS_M2 msg: 0.13 phi; -0.12 theta; 5.43 psi 2024-11-06T04:39:44Z- handleDepth()- Depth_Keller msg: 1730867984.16 epoch sec; seqNo:661; depth 43.89 2024-11-06T04:39:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867984.17 epoch sec; seqNo:661; 4061626.12 north; 594282.38 east; 36.70 lat; -121.94 lon; 43.89 depth; utmZone: 10 2024-11-06T04:39:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867982.16 2024-11-06T04:39:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:44Z- waiting for 1730867986.16, time is 1730867984.17 2024-11-06T04:39:44Z- AHRS_M2 msg: 1730867984.56 epoch sec; seqNo:610 2024-11-06T04:39:44Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.43 psi 2024-11-06T04:39:44Z- handleDepth()- Depth_Keller msg: 1730867984.57 epoch sec; seqNo:662; depth 43.93 2024-11-06T04:39:44Z- handleDVL() - DVL msg epochMillisec: 1730867984.59 2024-11-06T04:39:44Z- handleDvl() - RDI_Pathfinder msg: 1730867984.59 sec; seqNo:228; #beams:4 2024-11-06T04:39:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 53.34 , 46.19 , 47.15 , 52.62 2024-11-06T04:39:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.07 , -0.00 2024-11-06T04:39:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867984.60 epoch sec; seqNo:662; 4061626.12 north; 594282.38 east; 36.70 lat; -121.94 lon; 43.93 depth; utmZone: 10 2024-11-06T04:39:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867982.16 2024-11-06T04:39:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:44Z- waiting for 1730867986.16, time is 1730867984.60 2024-11-06T04:39:44Z- AHRS_M2 msg: 1730867984.96 epoch sec; seqNo:611 2024-11-06T04:39:44Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.43 psi 2024-11-06T04:39:44Z- handleDepth()- Depth_Keller msg: 1730867984.98 epoch sec; seqNo:663; depth 43.95 2024-11-06T04:39:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867984.99 epoch sec; seqNo:663; 4061626.48 north; 594281.15 east; 36.70 lat; -121.94 lon; 43.95 depth; utmZone: 10 2024-11-06T04:39:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867982.16 2024-11-06T04:39:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:45Z- waiting for 1730867986.16, time is 1730867984.99 2024-11-06T04:39:45Z- AHRS_M2 msg: 1730867985.37 epoch sec; seqNo:612 2024-11-06T04:39:45Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.43 psi 2024-11-06T04:39:45Z- handleDepth()- Depth_Keller msg: 1730867985.38 epoch sec; seqNo:664; depth 44.00 2024-11-06T04:39:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867985.39 epoch sec; seqNo:664; 4061626.48 north; 594281.15 east; 36.70 lat; -121.94 lon; 44.00 depth; utmZone: 10 2024-11-06T04:39:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867982.16 2024-11-06T04:39:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:45Z- waiting for 1730867986.16, time is 1730867985.39 2024-11-06T04:39:45Z- AHRS_M2 msg: 1730867985.78 epoch sec; seqNo:613 2024-11-06T04:39:45Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.43 psi 2024-11-06T04:39:45Z- handleDepth()- Depth_Keller msg: 1730867985.78 epoch sec; seqNo:665; depth 44.03 2024-11-06T04:39:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867985.79 epoch sec; seqNo:665; 4061626.86 north; 594281.15 east; 36.70 lat; -121.94 lon; 44.03 depth; utmZone: 10 2024-11-06T04:39:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867982.16 2024-11-06T04:39:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:45Z- waiting for 1730867986.16, time is 1730867985.79 2024-11-06T04:39:46Z- AHRS_M2 msg: 1730867986.18 epoch sec; seqNo:614 2024-11-06T04:39:46Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.43 psi 2024-11-06T04:39:46Z- handleDepth()- Depth_Keller msg: 1730867986.18 epoch sec; seqNo:666; depth 44.06 2024-11-06T04:39:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867986.20 epoch sec; seqNo:666; 4061627.24 north; 594281.15 east; 36.70 lat; -121.94 lon; 44.06 depth; utmZone: 10 2024-11-06T04:39:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867982.16 2024-11-06T04:39:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:46Z- waiting for 1730867986.16, time is 1730867986.20 2024-11-06T04:39:46Z- LcmTrn::updateTrn() >>>> pose time 1730867986.20, meas time 1730867984.59 <<<< 2024-11-06T04:39:46Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867986.20 2024-11-06T04:39:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867984.59 sec, ping # 0; waiting for INS data more recent than 1730867982.16... 2024-11-06T04:39:46Z- TerrainNav::motionUpdate() - currEstimate = (4061627, 594281.15, 44.06) 2024-11-06T04:39:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:39:46Z- TerrainNav:: SubcloudNIS is 0.13, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:39:46Z- TNavPF::Projecting Measurements 2024-11-06T04:39:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:39:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:39:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:39:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:39:46Z- TNavFilter::4 good beams. 2024-11-06T04:39:46Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:39:46Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:39:46Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:39:46Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:39:46Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:39:46Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:39:46Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:39:46Z- TerrainMapOctree: (vehN, vehE ) = (4061626.19, 594282.13). 2024-11-06T04:39:46Z- TerrainMapOctree: Min Distance = 127.91. 2024-11-06T04:39:46Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:39:46Z- Forcing Subcloud Comparison 2024-11-06T04:39:46Z- Weighting particles with subcloud comparison 2024-11-06T04:39:46Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:39:46Z- meanExpectedMeasDiff: -0.247426 delta_rms_squared: 1.803644 alpha: 0.989033 2024-11-06T04:39:46Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:39:46Z- meanExpectedMeasDiff: -0.124876 delta_rms_squared: 1.265118 alpha: 0.984437 2024-11-06T04:39:46Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:39:46Z- meanExpectedMeasDiff: 0.803101 delta_rms_squared: 1.346559 alpha: 0.985365 2024-11-06T04:39:46Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:39:46Z- meanExpectedMeasDiff: -0.057861 delta_rms_squared: 1.055067 alpha: 0.981397 2024-11-06T04:39:46Z- Subcloud NIS: 0.121290 2024-11-06T04:39:46Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:39:46Z- TNavPF:: sumWeights = 0.670322 2024-11-06T04:39:46Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:39:46Z- TNavPF::First Term of Map Covariance Matrix is 0.64 2024-11-06T04:39:46Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T04:39:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867984.59 sec. 2024-11-06T04:39:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:39:46Z- variances([0], [2], and psi_berg) = 0.100955 0.094803 0.000000 2024-11-06T04:39:46Z- publishEstimatesDecoder() - at 1730867986.20 sec 2024-11-06T04:39:46Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:39:46Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061571.10 594286.12 51.91 2024-11-06T04:39:46Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061571.20 594285.25 51.39 lat/lon: 36.70 -121.94 2024-11-06T04:39:46Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.10 0.09 0.85 0.00 2024-11-06T04:39:46Z- AHRS_M2 msg: 1730867986.58 epoch sec; seqNo:615 2024-11-06T04:39:46Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.43 psi 2024-11-06T04:39:46Z- handleDepth()- Depth_Keller msg: 1730867986.60 epoch sec; seqNo:667; depth 44.09 2024-11-06T04:39:46Z- handleDVL() - DVL msg epochMillisec: 1730867986.64 2024-11-06T04:39:46Z- handleDvl() - RDI_Pathfinder msg: 1730867986.64 sec; seqNo:229; #beams:4 2024-11-06T04:39:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 52.85 , 45.95 , 46.90 , 52.38 2024-11-06T04:39:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.10 , -0.03 2024-11-06T04:39:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867986.65 epoch sec; seqNo:667; 4061627.23 north; 594279.93 east; 36.70 lat; -121.94 lon; 44.09 depth; utmZone: 10 2024-11-06T04:39:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867986.20 2024-11-06T04:39:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:46Z- waiting for 1730867990.20, time is 1730867986.65 2024-11-06T04:39:46Z- AHRS_M2 msg: 1730867986.98 epoch sec; seqNo:616 2024-11-06T04:39:46Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.43 psi 2024-11-06T04:39:46Z- handleDepth()- Depth_Keller msg: 1730867986.99 epoch sec; seqNo:668; depth 44.13 2024-11-06T04:39:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867987.01 epoch sec; seqNo:668; 4061627.61 north; 594279.92 east; 36.70 lat; -121.94 lon; 44.13 depth; utmZone: 10 2024-11-06T04:39:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867986.20 2024-11-06T04:39:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:47Z- waiting for 1730867990.20, time is 1730867987.01 2024-11-06T04:39:47Z- AHRS_M2 msg: 1730867987.40 epoch sec; seqNo:617 2024-11-06T04:39:47Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.43 psi 2024-11-06T04:39:47Z- handleDepth()- Depth_Keller msg: 1730867987.41 epoch sec; seqNo:669; depth 44.17 2024-11-06T04:39:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867987.42 epoch sec; seqNo:669; 4061627.61 north; 594279.92 east; 36.70 lat; -121.94 lon; 44.17 depth; utmZone: 10 2024-11-06T04:39:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867986.20 2024-11-06T04:39:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:47Z- waiting for 1730867990.20, time is 1730867987.42 2024-11-06T04:39:47Z- AHRS_M2 msg: 1730867987.79 epoch sec; seqNo:618 2024-11-06T04:39:47Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.43 psi 2024-11-06T04:39:47Z- handleDepth()- Depth_Keller msg: 1730867987.80 epoch sec; seqNo:670; depth 44.21 2024-11-06T04:39:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867987.81 epoch sec; seqNo:670; 4061627.97 north; 594278.70 east; 36.70 lat; -121.94 lon; 44.21 depth; utmZone: 10 2024-11-06T04:39:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867986.20 2024-11-06T04:39:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:47Z- waiting for 1730867990.20, time is 1730867987.81 2024-11-06T04:39:48Z- handleDepth()- Depth_Keller msg: 1730867988.22 epoch sec; seqNo:671; depth 44.25 2024-11-06T04:39:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867988.23 epoch sec; seqNo:671; 4061628.35 north; 594278.69 east; 36.70 lat; -121.94 lon; 44.25 depth; utmZone: 10 2024-11-06T04:39:48Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:39:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867986.20 2024-11-06T04:39:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:48Z- waiting for 1730867990.20, time is 1730867988.23 2024-11-06T04:39:48Z- AHRS_M2 msg: 1730867988.60 epoch sec; seqNo:619 2024-11-06T04:39:48Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.42 psi 2024-11-06T04:39:48Z- handleDepth()- Depth_Keller msg: 1730867988.61 epoch sec; seqNo:672; depth 44.26 2024-11-06T04:39:48Z- handleDVL() - DVL msg epochMillisec: 1730867988.63 2024-11-06T04:39:48Z- handleDvl() - RDI_Pathfinder msg: 1730867988.63 sec; seqNo:230; #beams:4 2024-11-06T04:39:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 52.62 , 45.95 , 46.66 , 51.90 2024-11-06T04:39:48Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.10 , 0.01 2024-11-06T04:39:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867988.64 epoch sec; seqNo:672; 4061628.35 north; 594278.69 east; 36.70 lat; -121.94 lon; 44.26 depth; utmZone: 10 2024-11-06T04:39:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867986.20 2024-11-06T04:39:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:48Z- waiting for 1730867990.20, time is 1730867988.64 2024-11-06T04:39:49Z- AHRS_M2 msg: 1730867989.00 epoch sec; seqNo:620 2024-11-06T04:39:49Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.42 psi 2024-11-06T04:39:49Z- handleDepth()- Depth_Keller msg: 1730867989.01 epoch sec; seqNo:673; depth 44.32 2024-11-06T04:39:49Z- handleDVL() - DVL msg epochMillisec: 1730867989.01 2024-11-06T04:39:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:49Z- handleDvl() - RDI_Pathfinder msg: 1730867989.01 sec; seqNo:231; #beams:0 2024-11-06T04:39:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:49Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.10 , 0.01 2024-11-06T04:39:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867989.04 epoch sec; seqNo:673; 4061628.73 north; 594278.69 east; 36.70 lat; -121.94 lon; 44.32 depth; utmZone: 10 2024-11-06T04:39:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867986.20 2024-11-06T04:39:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:49Z- waiting for 1730867990.20, time is 1730867989.04 2024-11-06T04:39:49Z- AHRS_M2 msg: 1730867989.41 epoch sec; seqNo:621 2024-11-06T04:39:49Z- AHRS_M2 msg: 0.11 phi; -0.12 theta; 5.42 psi 2024-11-06T04:39:49Z- handleDepth()- Depth_Keller msg: 1730867989.42 epoch sec; seqNo:674; depth 44.36 2024-11-06T04:39:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867989.43 epoch sec; seqNo:674; 4061628.72 north; 594277.47 east; 36.70 lat; -121.94 lon; 44.36 depth; utmZone: 10 2024-11-06T04:39:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867986.20 2024-11-06T04:39:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:49Z- waiting for 1730867990.20, time is 1730867989.43 2024-11-06T04:39:49Z- AHRS_M2 msg: 1730867989.83 epoch sec; seqNo:622 2024-11-06T04:39:49Z- AHRS_M2 msg: 0.11 phi; -0.12 theta; 5.42 psi 2024-11-06T04:39:49Z- handleDepth()- Depth_Keller msg: 1730867989.84 epoch sec; seqNo:675; depth 44.40 2024-11-06T04:39:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867989.87 epoch sec; seqNo:675; 4061629.09 north; 594277.46 east; 36.70 lat; -121.94 lon; 44.40 depth; utmZone: 10 2024-11-06T04:39:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867986.20 2024-11-06T04:39:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:49Z- waiting for 1730867990.20, time is 1730867989.87 2024-11-06T04:39:50Z- AHRS_M2 msg: 1730867990.22 epoch sec; seqNo:623 2024-11-06T04:39:50Z- AHRS_M2 msg: 0.11 phi; -0.12 theta; 5.42 psi 2024-11-06T04:39:50Z- handleDepth()- Depth_Keller msg: 1730867990.22 epoch sec; seqNo:676; depth 44.43 2024-11-06T04:39:50Z- handleDVL() - DVL msg epochMillisec: 1730867990.23 2024-11-06T04:39:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:50Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:50Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:50Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:50Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:50Z- handleDvl() - RDI_Pathfinder msg: 1730867990.23 sec; seqNo:232; #beams:0 2024-11-06T04:39:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:50Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.10 , 0.01 2024-11-06T04:39:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867990.24 epoch sec; seqNo:676; 4061629.47 north; 594277.46 east; 36.70 lat; -121.94 lon; 44.43 depth; utmZone: 10 2024-11-06T04:39:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867986.20 2024-11-06T04:39:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:50Z- waiting for 1730867990.20, time is 1730867990.24 2024-11-06T04:39:50Z- LcmTrn::updateTrn() >>>> pose time 1730867990.24, meas time 1730867990.23 <<<< 2024-11-06T04:39:50Z- LcmTrn::updateTrn() equating pose 1730867990.23 and meas times 1730867990.23 2024-11-06T04:39:50Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730867990.23 2024-11-06T04:39:50Z- TerrainNav::motionUpdate() - currEstimate = (4061629, 594277.46, 44.43) 2024-11-06T04:39:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:39:50Z- TerrainNav:: SubcloudNIS is 0.12, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:39:50Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:39:50Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:39:50Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:39:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:39:50Z- variances([0], [2], and psi_berg) = 0.103669 0.098242 0.000000 2024-11-06T04:39:50Z- publishEstimatesDecoder() - at 1730867990.23 sec 2024-11-06T04:39:50Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:39:50Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061573.22 594282.43 52.19 2024-11-06T04:39:50Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061573.44 594281.57 51.77 lat/lon: 36.70 -121.94 2024-11-06T04:39:50Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.10 0.10 1.09 0.00 2024-11-06T04:39:50Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:39:50Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:39:50Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:39:50Z- AHRS_M2 msg: 1730867990.61 epoch sec; seqNo:624 2024-11-06T04:39:50Z- AHRS_M2 msg: 0.11 phi; -0.12 theta; 5.42 psi 2024-11-06T04:39:50Z- handleDepth()- Depth_Keller msg: 1730867990.64 epoch sec; seqNo:677; depth 44.48 2024-11-06T04:39:50Z- handleDVL() - DVL msg epochMillisec: 1730867990.67 2024-11-06T04:39:50Z- handleDvl() - RDI_Pathfinder msg: 1730867990.67 sec; seqNo:233; #beams:4 2024-11-06T04:39:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 52.38 , 45.95 , 46.90 , 52.14 2024-11-06T04:39:50Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.09 , 0.07 2024-11-06T04:39:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867990.68 epoch sec; seqNo:677; 4061629.47 north; 594277.46 east; 36.70 lat; -121.94 lon; 44.48 depth; utmZone: 10 2024-11-06T04:39:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867990.23 2024-11-06T04:39:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:50Z- waiting for 1730867994.23, time is 1730867990.68 2024-11-06T04:39:51Z- AHRS_M2 msg: 1730867991.02 epoch sec; seqNo:625 2024-11-06T04:39:51Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.42 psi 2024-11-06T04:39:51Z- handleDepth()- Depth_Keller msg: 1730867991.03 epoch sec; seqNo:678; depth 44.52 2024-11-06T04:39:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867991.05 epoch sec; seqNo:678; 4061629.84 north; 594276.24 east; 36.70 lat; -121.94 lon; 44.52 depth; utmZone: 10 2024-11-06T04:39:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867990.23 2024-11-06T04:39:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:51Z- waiting for 1730867994.23, time is 1730867991.05 2024-11-06T04:39:51Z- AHRS_M2 msg: 1730867991.43 epoch sec; seqNo:626 2024-11-06T04:39:51Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.42 psi 2024-11-06T04:39:51Z- handleDepth()- Depth_Keller msg: 1730867991.44 epoch sec; seqNo:679; depth 44.56 2024-11-06T04:39:51Z- handleDVL() - DVL msg epochMillisec: 1730867991.44 2024-11-06T04:39:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:51Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:51Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:51Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:51Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:51Z- handleDvl() - RDI_Pathfinder msg: 1730867991.44 sec; seqNo:234; #beams:0 2024-11-06T04:39:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:51Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.09 , 0.07 2024-11-06T04:39:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867991.45 epoch sec; seqNo:679; 4061629.84 north; 594276.24 east; 36.70 lat; -121.94 lon; 44.56 depth; utmZone: 10 2024-11-06T04:39:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867990.23 2024-11-06T04:39:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:51Z- waiting for 1730867994.23, time is 1730867991.45 2024-11-06T04:39:51Z- AHRS_M2 msg: 1730867991.85 epoch sec; seqNo:627 2024-11-06T04:39:51Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.42 psi 2024-11-06T04:39:51Z- handleDepth()- Depth_Keller msg: 1730867991.85 epoch sec; seqNo:680; depth 44.59 2024-11-06T04:39:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867991.87 epoch sec; seqNo:680; 4061630.22 north; 594276.23 east; 36.70 lat; -121.94 lon; 44.59 depth; utmZone: 10 2024-11-06T04:39:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867990.23 2024-11-06T04:39:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:52Z- waiting for 1730867994.23, time is 1730867991.87 2024-11-06T04:39:52Z- AHRS_M2 msg: 1730867992.24 epoch sec; seqNo:628 2024-11-06T04:39:52Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.42 psi 2024-11-06T04:39:52Z- handleDepth()- Depth_Keller msg: 1730867992.25 epoch sec; seqNo:681; depth 44.64 2024-11-06T04:39:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867992.26 epoch sec; seqNo:681; 4061630.58 north; 594275.01 east; 36.70 lat; -121.94 lon; 44.64 depth; utmZone: 10 2024-11-06T04:39:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867990.23 2024-11-06T04:39:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:52Z- waiting for 1730867994.23, time is 1730867992.26 2024-11-06T04:39:52Z- AHRS_M2 msg: 1730867992.64 epoch sec; seqNo:629 2024-11-06T04:39:52Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.42 psi 2024-11-06T04:39:52Z- handleDepth()- Depth_Keller msg: 1730867992.65 epoch sec; seqNo:682; depth 44.66 2024-11-06T04:39:52Z- handleDVL() - DVL msg epochMillisec: 1730867992.67 2024-11-06T04:39:52Z- handleDvl() - RDI_Pathfinder msg: 1730867992.67 sec; seqNo:235; #beams:4 2024-11-06T04:39:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 53.09 , 45.71 , 46.90 , 52.14 2024-11-06T04:39:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.02 , 0.08 2024-11-06T04:39:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867992.68 epoch sec; seqNo:682; 4061630.58 north; 594275.01 east; 36.70 lat; -121.94 lon; 44.66 depth; utmZone: 10 2024-11-06T04:39:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867990.23 2024-11-06T04:39:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:52Z- waiting for 1730867994.23, time is 1730867992.68 2024-11-06T04:39:53Z- AHRS_M2 msg: 1730867993.05 epoch sec; seqNo:630 2024-11-06T04:39:53Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.42 psi 2024-11-06T04:39:53Z- handleDepth()- Depth_Keller msg: 1730867993.07 epoch sec; seqNo:683; depth 44.71 2024-11-06T04:39:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867993.08 epoch sec; seqNo:683; 4061630.96 north; 594275.00 east; 36.70 lat; -121.94 lon; 44.71 depth; utmZone: 10 2024-11-06T04:39:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867990.23 2024-11-06T04:39:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:53Z- waiting for 1730867994.23, time is 1730867993.08 2024-11-06T04:39:53Z- AHRS_M2 msg: 1730867993.45 epoch sec; seqNo:631 2024-11-06T04:39:53Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.42 psi 2024-11-06T04:39:53Z- handleDepth()- Depth_Keller msg: 1730867993.45 epoch sec; seqNo:684; depth 44.75 2024-11-06T04:39:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867993.46 epoch sec; seqNo:684; 4061631.34 north; 594275.00 east; 36.70 lat; -121.94 lon; 44.75 depth; utmZone: 10 2024-11-06T04:39:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867990.23 2024-11-06T04:39:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:53Z- waiting for 1730867994.23, time is 1730867993.46 2024-11-06T04:39:53Z- AHRS_M2 msg: 1730867993.85 epoch sec; seqNo:632 2024-11-06T04:39:53Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.42 psi 2024-11-06T04:39:53Z- handleDepth()- Depth_Keller msg: 1730867993.86 epoch sec; seqNo:685; depth 44.78 2024-11-06T04:39:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867993.87 epoch sec; seqNo:685; 4061631.34 north; 594275.00 east; 36.70 lat; -121.94 lon; 44.78 depth; utmZone: 10 2024-11-06T04:39:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867990.23 2024-11-06T04:39:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:54Z- waiting for 1730867994.23, time is 1730867993.87 2024-11-06T04:39:54Z- AHRS_M2 msg: 1730867994.26 epoch sec; seqNo:633 2024-11-06T04:39:54Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.42 psi 2024-11-06T04:39:54Z- handleDepth()- Depth_Keller msg: 1730867994.26 epoch sec; seqNo:686; depth 44.81 2024-11-06T04:39:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867994.28 epoch sec; seqNo:686; 4061631.71 north; 594273.77 east; 36.70 lat; -121.94 lon; 44.81 depth; utmZone: 10 2024-11-06T04:39:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867990.23 2024-11-06T04:39:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:54Z- waiting for 1730867994.23, time is 1730867994.28 2024-11-06T04:39:54Z- LcmTrn::updateTrn() >>>> pose time 1730867994.28, meas time 1730867992.67 <<<< 2024-11-06T04:39:54Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867994.28 2024-11-06T04:39:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867992.67 sec, ping # 0; waiting for INS data more recent than 1730867990.23... 2024-11-06T04:39:54Z- TerrainNav::motionUpdate() - currEstimate = (4061632, 594273.77, 44.81) 2024-11-06T04:39:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:39:54Z- TerrainNav:: SubcloudNIS is 0.12, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:39:54Z- TNavPF::Projecting Measurements 2024-11-06T04:39:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:39:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:39:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:39:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:39:54Z- TNavFilter::4 good beams. 2024-11-06T04:39:54Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:39:54Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:39:54Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:39:54Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:39:54Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:39:54Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:39:54Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:39:54Z- TerrainMapOctree: (vehN, vehE ) = (4061630.82, 594275.24). 2024-11-06T04:39:54Z- TerrainMapOctree: Min Distance = 121.83. 2024-11-06T04:39:54Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:39:54Z- Forcing Subcloud Comparison 2024-11-06T04:39:54Z- Weighting particles with subcloud comparison 2024-11-06T04:39:54Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:39:54Z- meanExpectedMeasDiff: 0.677144 delta_rms_squared: 2.098702 alpha: 0.990560 2024-11-06T04:39:54Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:39:54Z- meanExpectedMeasDiff: -0.044848 delta_rms_squared: 1.596279 alpha: 0.987626 2024-11-06T04:39:54Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:39:54Z- meanExpectedMeasDiff: 1.390327 delta_rms_squared: 1.580491 alpha: 0.987504 2024-11-06T04:39:54Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:39:54Z- meanExpectedMeasDiff: -0.601527 delta_rms_squared: 2.124987 alpha: 0.990676 2024-11-06T04:39:54Z- Subcloud NIS: 0.121061 2024-11-06T04:39:54Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:39:54Z- TNavPF:: sumWeights = 0.527582 2024-11-06T04:39:54Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:39:54Z- TNavPF::First Term of Map Covariance Matrix is 0.51 2024-11-06T04:39:54Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T04:39:54Z- TerrainNav::Resampling particle filter... 2024-11-06T04:39:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867992.67 sec. 2024-11-06T04:39:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:39:54Z- variances([0], [2], and psi_berg) = 0.106000 0.100132 0.000000 2024-11-06T04:39:54Z- publishEstimatesDecoder() - at 1730867994.28 sec 2024-11-06T04:39:54Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:39:54Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061575.54 594277.90 49.97 2024-11-06T04:39:54Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061575.67 594277.88 51.98 lat/lon: 36.70 -121.94 2024-11-06T04:39:54Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.11 0.10 0.84 0.00 2024-11-06T04:39:54Z- AHRS_M2 msg: 1730867994.66 epoch sec; seqNo:634 2024-11-06T04:39:54Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.42 psi 2024-11-06T04:39:54Z- handleDepth()- Depth_Keller msg: 1730867994.66 epoch sec; seqNo:687; depth 44.85 2024-11-06T04:39:54Z- handleDVL() - DVL msg epochMillisec: 1730867994.70 2024-11-06T04:39:54Z- handleDvl() - RDI_Pathfinder msg: 1730867994.70 sec; seqNo:236; #beams:4 2024-11-06T04:39:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 52.85 , 45.47 , 46.66 , 52.14 2024-11-06T04:39:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , 0.05 2024-11-06T04:39:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867994.72 epoch sec; seqNo:687; 4061632.09 north; 594273.77 east; 36.70 lat; -121.94 lon; 44.85 depth; utmZone: 10 2024-11-06T04:39:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867994.28 2024-11-06T04:39:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:54Z- waiting for 1730867998.28, time is 1730867994.72 2024-11-06T04:39:55Z- AHRS_M2 msg: 1730867995.07 epoch sec; seqNo:635 2024-11-06T04:39:55Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.42 psi 2024-11-06T04:39:55Z- handleDepth()- Depth_Keller msg: 1730867995.07 epoch sec; seqNo:688; depth 44.88 2024-11-06T04:39:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867995.09 epoch sec; seqNo:688; 4061632.09 north; 594273.77 east; 36.70 lat; -121.94 lon; 44.88 depth; utmZone: 10 2024-11-06T04:39:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867994.28 2024-11-06T04:39:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:55Z- waiting for 1730867998.28, time is 1730867995.09 2024-11-06T04:39:55Z- AHRS_M2 msg: 1730867995.47 epoch sec; seqNo:636 2024-11-06T04:39:55Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.42 psi 2024-11-06T04:39:55Z- handleDepth()- Depth_Keller msg: 1730867995.48 epoch sec; seqNo:689; depth 44.91 2024-11-06T04:39:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867995.49 epoch sec; seqNo:689; 4061632.46 north; 594273.77 east; 36.70 lat; -121.94 lon; 44.91 depth; utmZone: 10 2024-11-06T04:39:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867994.28 2024-11-06T04:39:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:55Z- waiting for 1730867998.28, time is 1730867995.49 2024-11-06T04:39:55Z- AHRS_M2 msg: 1730867995.87 epoch sec; seqNo:637 2024-11-06T04:39:55Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.42 psi 2024-11-06T04:39:55Z- handleDepth()- Depth_Keller msg: 1730867995.89 epoch sec; seqNo:690; depth 44.94 2024-11-06T04:39:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867995.91 epoch sec; seqNo:690; 4061632.45 north; 594272.55 east; 36.70 lat; -121.94 lon; 44.94 depth; utmZone: 10 2024-11-06T04:39:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867994.28 2024-11-06T04:39:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:56Z- waiting for 1730867998.28, time is 1730867995.91 2024-11-06T04:39:56Z- AHRS_M2 msg: 1730867996.28 epoch sec; seqNo:638 2024-11-06T04:39:56Z- AHRS_M2 msg: 0.11 phi; -0.13 theta; 5.42 psi 2024-11-06T04:39:56Z- handleDepth()- Depth_Keller msg: 1730867996.29 epoch sec; seqNo:691; depth 44.98 2024-11-06T04:39:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867996.30 epoch sec; seqNo:691; 4061632.83 north; 594272.54 east; 36.70 lat; -121.94 lon; 44.98 depth; utmZone: 10 2024-11-06T04:39:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867994.28 2024-11-06T04:39:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:56Z- waiting for 1730867998.28, time is 1730867996.30 2024-11-06T04:39:56Z- AHRS_M2 msg: 1730867996.69 epoch sec; seqNo:639 2024-11-06T04:39:56Z- AHRS_M2 msg: 0.11 phi; -0.13 theta; 5.42 psi 2024-11-06T04:39:56Z- handleDepth()- Depth_Keller msg: 1730867996.71 epoch sec; seqNo:692; depth 44.99 2024-11-06T04:39:56Z- handleDVL() - DVL msg epochMillisec: 1730867996.73 2024-11-06T04:39:56Z- handleDvl() - RDI_Pathfinder msg: 1730867996.73 sec; seqNo:237; #beams:4 2024-11-06T04:39:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 52.62 , 45.71 , 46.43 , 51.90 2024-11-06T04:39:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , -0.02 2024-11-06T04:39:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867996.74 epoch sec; seqNo:692; 4061633.21 north; 594272.54 east; 36.70 lat; -121.94 lon; 44.99 depth; utmZone: 10 2024-11-06T04:39:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867994.28 2024-11-06T04:39:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:56Z- waiting for 1730867998.28, time is 1730867996.74 2024-11-06T04:39:57Z- AHRS_M2 msg: 1730867997.08 epoch sec; seqNo:640 2024-11-06T04:39:57Z- AHRS_M2 msg: 0.12 phi; -0.14 theta; 5.42 psi 2024-11-06T04:39:57Z- handleDepth()- Depth_Keller msg: 1730867997.09 epoch sec; seqNo:693; depth 45.04 2024-11-06T04:39:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867997.11 epoch sec; seqNo:693; 4061633.21 north; 594272.54 east; 36.70 lat; -121.94 lon; 45.04 depth; utmZone: 10 2024-11-06T04:39:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867994.28 2024-11-06T04:39:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:57Z- waiting for 1730867998.28, time is 1730867997.11 2024-11-06T04:39:57Z- AHRS_M2 msg: 1730867997.49 epoch sec; seqNo:641 2024-11-06T04:39:57Z- AHRS_M2 msg: 0.12 phi; -0.14 theta; 5.42 psi 2024-11-06T04:39:57Z- handleDepth()- Depth_Keller msg: 1730867997.49 epoch sec; seqNo:694; depth 45.06 2024-11-06T04:39:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867997.50 epoch sec; seqNo:694; 4061633.59 north; 594272.53 east; 36.70 lat; -121.94 lon; 45.06 depth; utmZone: 10 2024-11-06T04:39:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867994.28 2024-11-06T04:39:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:57Z- waiting for 1730867998.28, time is 1730867997.50 2024-11-06T04:39:57Z- AHRS_M2 msg: 1730867997.89 epoch sec; seqNo:642 2024-11-06T04:39:57Z- AHRS_M2 msg: 0.11 phi; -0.14 theta; 5.42 psi 2024-11-06T04:39:57Z- handleDepth()- Depth_Keller msg: 1730867997.89 epoch sec; seqNo:695; depth 45.08 2024-11-06T04:39:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867997.91 epoch sec; seqNo:695; 4061633.95 north; 594271.31 east; 36.70 lat; -121.94 lon; 45.08 depth; utmZone: 10 2024-11-06T04:39:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867994.28 2024-11-06T04:39:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:58Z- waiting for 1730867998.28, time is 1730867997.91 2024-11-06T04:39:58Z- AHRS_M2 msg: 1730867998.30 epoch sec; seqNo:643 2024-11-06T04:39:58Z- AHRS_M2 msg: 0.11 phi; -0.14 theta; 5.42 psi 2024-11-06T04:39:58Z- handleDepth()- Depth_Keller msg: 1730867998.31 epoch sec; seqNo:696; depth 45.11 2024-11-06T04:39:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867998.34 epoch sec; seqNo:696; 4061633.95 north; 594271.31 east; 36.70 lat; -121.94 lon; 45.11 depth; utmZone: 10 2024-11-06T04:39:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867994.28 2024-11-06T04:39:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:58Z- waiting for 1730867998.28, time is 1730867998.34 2024-11-06T04:39:58Z- LcmTrn::updateTrn() >>>> pose time 1730867998.34, meas time 1730867996.73 <<<< 2024-11-06T04:39:58Z- LcmTrn::updateTrn() measUpdate first last update time: 1730867998.34 2024-11-06T04:39:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730867996.73 sec, ping # 0; waiting for INS data more recent than 1730867994.28... 2024-11-06T04:39:58Z- TerrainNav::motionUpdate() - currEstimate = (4061634, 594271.31, 45.11) 2024-11-06T04:39:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:39:58Z- TerrainNav:: SubcloudNIS is 0.12, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:39:58Z- TNavPF::Projecting Measurements 2024-11-06T04:39:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:39:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:39:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:39:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:39:58Z- TNavFilter::4 good beams. 2024-11-06T04:39:58Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:39:58Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:39:58Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:39:58Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:39:58Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:39:58Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:39:58Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:39:58Z- TerrainMapOctree: (vehN, vehE ) = (4061633.06, 594272.29). 2024-11-06T04:39:58Z- TerrainMapOctree: Min Distance = 119.04. 2024-11-06T04:39:58Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:39:58Z- Forcing Subcloud Comparison 2024-11-06T04:39:58Z- Weighting particles with subcloud comparison 2024-11-06T04:39:58Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:39:58Z- meanExpectedMeasDiff: 0.482481 delta_rms_squared: 1.725053 alpha: 0.988539 2024-11-06T04:39:58Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:39:58Z- meanExpectedMeasDiff: 0.032760 delta_rms_squared: 1.322038 alpha: 0.985097 2024-11-06T04:39:58Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:39:58Z- meanExpectedMeasDiff: 0.326194 delta_rms_squared: 1.640036 alpha: 0.987952 2024-11-06T04:39:58Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:39:58Z- meanExpectedMeasDiff: -1.008707 delta_rms_squared: 1.777170 alpha: 0.988871 2024-11-06T04:39:58Z- Subcloud NIS: 0.121223 2024-11-06T04:39:58Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:39:58Z- TNavPF:: sumWeights = 0.612102 2024-11-06T04:39:58Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:39:58Z- TNavPF::First Term of Map Covariance Matrix is 0.52 2024-11-06T04:39:58Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T04:39:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730867996.73 sec. 2024-11-06T04:39:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:39:58Z- variances([0], [2], and psi_berg) = 0.106209 0.100944 0.000000 2024-11-06T04:39:58Z- publishEstimatesDecoder() - at 1730867998.34 sec 2024-11-06T04:39:58Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:39:58Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061577.68 594275.37 52.12 2024-11-06T04:39:58Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061577.92 594275.41 52.30 lat/lon: 36.70 -121.94 2024-11-06T04:39:58Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.11 0.10 0.76 0.00 2024-11-06T04:39:58Z- AHRS_M2 msg: 1730867998.72 epoch sec; seqNo:644 2024-11-06T04:39:58Z- AHRS_M2 msg: 0.11 phi; -0.14 theta; 5.42 psi 2024-11-06T04:39:58Z- handleDepth()- Depth_Keller msg: 1730867998.74 epoch sec; seqNo:697; depth 45.15 2024-11-06T04:39:58Z- handleDVL() - DVL msg epochMillisec: 1730867998.78 2024-11-06T04:39:58Z- handleDvl() - RDI_Pathfinder msg: 1730867998.78 sec; seqNo:238; #beams:4 2024-11-06T04:39:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 52.37 , 45.47 , 45.71 , 51.90 2024-11-06T04:39:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.08 , -0.05 2024-11-06T04:39:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867998.81 epoch sec; seqNo:697; 4061634.33 north; 594271.30 east; 36.70 lat; -121.94 lon; 45.15 depth; utmZone: 10 2024-11-06T04:39:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:39:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867998.34 2024-11-06T04:39:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:58Z- waiting for 1730868002.34, time is 1730867998.81 2024-11-06T04:39:59Z- handleDepth()- Depth_Keller msg: 1730867999.13 epoch sec; seqNo:698; depth 45.18 2024-11-06T04:39:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867999.15 epoch sec; seqNo:698; 4061634.33 north; 594271.30 east; 36.70 lat; -121.94 lon; 45.18 depth; utmZone: 10 2024-11-06T04:39:59Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:39:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867998.34 2024-11-06T04:39:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:59Z- waiting for 1730868002.34, time is 1730867999.15 2024-11-06T04:39:59Z- AHRS_M2 msg: 1730867999.51 epoch sec; seqNo:645 2024-11-06T04:39:59Z- AHRS_M2 msg: 0.11 phi; -0.15 theta; 5.42 psi 2024-11-06T04:39:59Z- handleDepth()- Depth_Keller msg: 1730867999.53 epoch sec; seqNo:699; depth 45.19 2024-11-06T04:39:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867999.54 epoch sec; seqNo:699; 4061634.70 north; 594270.08 east; 36.70 lat; -121.94 lon; 45.19 depth; utmZone: 10 2024-11-06T04:39:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:39:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867998.34 2024-11-06T04:39:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:39:59Z- waiting for 1730868002.34, time is 1730867999.54 2024-11-06T04:39:59Z- AHRS_M2 msg: 1730867999.91 epoch sec; seqNo:646 2024-11-06T04:39:59Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.42 psi 2024-11-06T04:39:59Z- handleDepth()- Depth_Keller msg: 1730867999.92 epoch sec; seqNo:700; depth 45.23 2024-11-06T04:39:59Z- handleDVL() - DVL msg epochMillisec: 1730867999.92 2024-11-06T04:39:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:39:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:39:59Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:39:59Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:39:59Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:39:59Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:39:59Z- handleDvl() - RDI_Pathfinder msg: 1730867999.92 sec; seqNo:239; #beams:0 2024-11-06T04:39:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:39:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.08 , -0.05 2024-11-06T04:39:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730867999.94 epoch sec; seqNo:700; 4061635.08 north; 594270.08 east; 36.70 lat; -121.94 lon; 45.23 depth; utmZone: 10 2024-11-06T04:40:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867998.34 2024-11-06T04:40:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:00Z- waiting for 1730868002.34, time is 1730867999.94 2024-11-06T04:40:00Z- AHRS_M2 msg: 1730868000.32 epoch sec; seqNo:647 2024-11-06T04:40:00Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.42 psi 2024-11-06T04:40:00Z- handleDepth()- Depth_Keller msg: 1730868000.32 epoch sec; seqNo:701; depth 45.27 2024-11-06T04:40:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868000.33 epoch sec; seqNo:701; 4061635.08 north; 594270.08 east; 36.70 lat; -121.94 lon; 45.27 depth; utmZone: 10 2024-11-06T04:40:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867998.34 2024-11-06T04:40:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:00Z- waiting for 1730868002.34, time is 1730868000.33 2024-11-06T04:40:00Z- AHRS_M2 msg: 1730868000.72 epoch sec; seqNo:648 2024-11-06T04:40:00Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.42 psi 2024-11-06T04:40:00Z- handleDepth()- Depth_Keller msg: 1730868000.73 epoch sec; seqNo:702; depth 45.30 2024-11-06T04:40:00Z- handleDVL() - DVL msg epochMillisec: 1730868000.75 2024-11-06T04:40:00Z- handleDvl() - RDI_Pathfinder msg: 1730868000.75 sec; seqNo:240; #beams:4 2024-11-06T04:40:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 52.17 , 45.32 , 45.54 , 52.17 2024-11-06T04:40:00Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.11 , 0.00 2024-11-06T04:40:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868000.76 epoch sec; seqNo:702; 4061635.44 north; 594268.85 east; 36.70 lat; -121.94 lon; 45.30 depth; utmZone: 10 2024-11-06T04:40:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867998.34 2024-11-06T04:40:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:00Z- waiting for 1730868002.34, time is 1730868000.76 2024-11-06T04:40:01Z- AHRS_M2 msg: 1730868001.12 epoch sec; seqNo:649 2024-11-06T04:40:01Z- AHRS_M2 msg: 0.13 phi; -0.15 theta; 5.42 psi 2024-11-06T04:40:01Z- handleDepth()- Depth_Keller msg: 1730868001.14 epoch sec; seqNo:703; depth 45.34 2024-11-06T04:40:01Z- handleDVL() - DVL msg epochMillisec: 1730868001.14 2024-11-06T04:40:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:01Z- handleDvl() - RDI_Pathfinder msg: 1730868001.14 sec; seqNo:241; #beams:0 2024-11-06T04:40:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:01Z- handleDvl() - RDI_Pathfinder msg: velocities 1.00 , -0.11 , 0.00 2024-11-06T04:40:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868001.16 epoch sec; seqNo:703; 4061635.44 north; 594268.85 east; 36.70 lat; -121.94 lon; 45.34 depth; utmZone: 10 2024-11-06T04:40:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867998.34 2024-11-06T04:40:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:01Z- waiting for 1730868002.34, time is 1730868001.16 2024-11-06T04:40:01Z- AHRS_M2 msg: 1730868001.54 epoch sec; seqNo:650 2024-11-06T04:40:01Z- AHRS_M2 msg: 0.13 phi; -0.16 theta; 5.42 psi 2024-11-06T04:40:01Z- handleDepth()- Depth_Keller msg: 1730868001.56 epoch sec; seqNo:704; depth 45.37 2024-11-06T04:40:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868001.57 epoch sec; seqNo:704; 4061635.82 north; 594268.85 east; 36.70 lat; -121.94 lon; 45.37 depth; utmZone: 10 2024-11-06T04:40:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867998.34 2024-11-06T04:40:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:01Z- waiting for 1730868002.34, time is 1730868001.57 2024-11-06T04:40:01Z- AHRS_M2 msg: 1730868001.93 epoch sec; seqNo:651 2024-11-06T04:40:01Z- AHRS_M2 msg: 0.13 phi; -0.16 theta; 5.43 psi 2024-11-06T04:40:01Z- handleDepth()- Depth_Keller msg: 1730868001.93 epoch sec; seqNo:705; depth 45.41 2024-11-06T04:40:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868001.94 epoch sec; seqNo:705; 4061636.20 north; 594268.84 east; 36.70 lat; -121.94 lon; 45.41 depth; utmZone: 10 2024-11-06T04:40:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867998.34 2024-11-06T04:40:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:02Z- waiting for 1730868002.34, time is 1730868001.94 2024-11-06T04:40:02Z- AHRS_M2 msg: 1730868002.34 epoch sec; seqNo:652 2024-11-06T04:40:02Z- AHRS_M2 msg: 0.13 phi; -0.16 theta; 5.43 psi 2024-11-06T04:40:02Z- handleDepth()- Depth_Keller msg: 1730868002.38 epoch sec; seqNo:706; depth 45.45 2024-11-06T04:40:02Z- handleDVL() - DVL msg epochMillisec: 1730868002.43 2024-11-06T04:40:02Z- handleDvl() - RDI_Pathfinder msg: 1730868002.43 sec; seqNo:242; #beams:4 2024-11-06T04:40:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 53.05 , 44.87 , 45.32 , 52.61 2024-11-06T04:40:02Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.09 , 0.08 2024-11-06T04:40:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868002.46 epoch sec; seqNo:706; 4061636.19 north; 594267.62 east; 36.70 lat; -121.94 lon; 45.45 depth; utmZone: 10 2024-11-06T04:40:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730867998.34 2024-11-06T04:40:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:02Z- waiting for 1730868002.34, time is 1730868002.46 2024-11-06T04:40:02Z- LcmTrn::updateTrn() >>>> pose time 1730868002.46, meas time 1730868002.43 <<<< 2024-11-06T04:40:02Z- LcmTrn::updateTrn() equating pose 1730868002.43 and meas times 1730868002.43 2024-11-06T04:40:02Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868002.43 2024-11-06T04:40:02Z- TerrainNav::motionUpdate() - currEstimate = (4061636, 594267.62, 45.45) 2024-11-06T04:40:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:40:02Z- TerrainNav:: SubcloudNIS is 0.12, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:40:02Z- TNavPF::Projecting Measurements 2024-11-06T04:40:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:40:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:40:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:40:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:40:02Z- TNavFilter::4 good beams. 2024-11-06T04:40:02Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:40:02Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:40:02Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:40:02Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:40:02Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:40:02Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:40:02Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:40:02Z- TerrainMapOctree: (vehN, vehE ) = (4061636.19, 594267.62). 2024-11-06T04:40:02Z- TerrainMapOctree: Min Distance = 115.17. 2024-11-06T04:40:02Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:40:02Z- Forcing Subcloud Comparison 2024-11-06T04:40:02Z- Weighting particles with subcloud comparison 2024-11-06T04:40:02Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:02Z- meanExpectedMeasDiff: 0.606335 delta_rms_squared: 1.841292 alpha: 0.989255 2024-11-06T04:40:02Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:02Z- meanExpectedMeasDiff: -0.136565 delta_rms_squared: 1.209167 alpha: 0.983729 2024-11-06T04:40:02Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:02Z- meanExpectedMeasDiff: 0.003353 delta_rms_squared: 1.167506 alpha: 0.983158 2024-11-06T04:40:02Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:02Z- meanExpectedMeasDiff: -0.888678 delta_rms_squared: 1.712409 alpha: 0.988455 2024-11-06T04:40:02Z- Subcloud NIS: 0.116641 2024-11-06T04:40:02Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:40:02Z- TNavPF:: sumWeights = 0.637296 2024-11-06T04:40:02Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:40:02Z- TNavPF::First Term of Map Covariance Matrix is 0.59 2024-11-06T04:40:02Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T04:40:02Z- TerrainNav::Resampling particle filter... 2024-11-06T04:40:02Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868002.43 sec, ping # 0. 2024-11-06T04:40:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:40:02Z- variances([0], [2], and psi_berg) = 0.108581 0.103567 0.000000 2024-11-06T04:40:02Z- publishEstimatesDecoder() - at 1730868002.43 sec 2024-11-06T04:40:02Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:40:02Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061580.19 594271.70 52.67 2024-11-06T04:40:02Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061580.15 594271.72 52.68 lat/lon: 36.70 -121.94 2024-11-06T04:40:02Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.11 0.10 0.50 0.00 2024-11-06T04:40:02Z- AHRS_M2 msg: 1730868002.74 epoch sec; seqNo:653 2024-11-06T04:40:02Z- AHRS_M2 msg: 0.13 phi; -0.16 theta; 5.43 psi 2024-11-06T04:40:02Z- handleDepth()- Depth_Keller msg: 1730868002.76 epoch sec; seqNo:707; depth 45.51 2024-11-06T04:40:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868002.79 epoch sec; seqNo:707; 4061636.56 north; 594267.62 east; 36.70 lat; -121.94 lon; 45.51 depth; utmZone: 10 2024-11-06T04:40:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868002.43 2024-11-06T04:40:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:02Z- waiting for 1730868006.43, time is 1730868002.79 2024-11-06T04:40:03Z- AHRS_M2 msg: 1730868003.14 epoch sec; seqNo:654 2024-11-06T04:40:03Z- AHRS_M2 msg: 0.13 phi; -0.17 theta; 5.42 psi 2024-11-06T04:40:03Z- handleDepth()- Depth_Keller msg: 1730868003.15 epoch sec; seqNo:708; depth 45.56 2024-11-06T04:40:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868003.16 epoch sec; seqNo:708; 4061636.56 north; 594267.62 east; 36.70 lat; -121.94 lon; 45.56 depth; utmZone: 10 2024-11-06T04:40:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868002.43 2024-11-06T04:40:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:03Z- waiting for 1730868006.43, time is 1730868003.16 2024-11-06T04:40:03Z- AHRS_M2 msg: 1730868003.55 epoch sec; seqNo:655 2024-11-06T04:40:03Z- AHRS_M2 msg: 0.13 phi; -0.17 theta; 5.43 psi 2024-11-06T04:40:03Z- handleDepth()- Depth_Keller msg: 1730868003.56 epoch sec; seqNo:709; depth 45.61 2024-11-06T04:40:03Z- handleDVL() - DVL msg epochMillisec: 1730868003.56 2024-11-06T04:40:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:03Z- handleDvl() - RDI_Pathfinder msg: 1730868003.56 sec; seqNo:243; #beams:0 2024-11-06T04:40:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:03Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.09 , 0.08 2024-11-06T04:40:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868003.57 epoch sec; seqNo:709; 4061636.94 north; 594267.61 east; 36.70 lat; -121.94 lon; 45.61 depth; utmZone: 10 2024-11-06T04:40:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868002.43 2024-11-06T04:40:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:03Z- waiting for 1730868006.43, time is 1730868003.57 2024-11-06T04:40:03Z- AHRS_M2 msg: 1730868003.96 epoch sec; seqNo:656 2024-11-06T04:40:03Z- AHRS_M2 msg: 0.12 phi; -0.17 theta; 5.42 psi 2024-11-06T04:40:03Z- handleDepth()- Depth_Keller msg: 1730868003.96 epoch sec; seqNo:710; depth 45.64 2024-11-06T04:40:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868003.98 epoch sec; seqNo:710; 4061637.31 north; 594266.39 east; 36.70 lat; -121.94 lon; 45.64 depth; utmZone: 10 2024-11-06T04:40:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868002.43 2024-11-06T04:40:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:04Z- waiting for 1730868006.43, time is 1730868003.98 2024-11-06T04:40:04Z- AHRS_M2 msg: 1730868004.39 epoch sec; seqNo:657 2024-11-06T04:40:04Z- AHRS_M2 msg: 0.12 phi; -0.17 theta; 5.42 psi 2024-11-06T04:40:04Z- handleDepth()- Depth_Keller msg: 1730868004.41 epoch sec; seqNo:711; depth 45.68 2024-11-06T04:40:04Z- handleDVL() - DVL msg epochMillisec: 1730868004.43 2024-11-06T04:40:04Z- handleDvl() - RDI_Pathfinder msg: 1730868004.43 sec; seqNo:244; #beams:4 2024-11-06T04:40:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 52.83 , 44.87 , 45.10 , 52.83 2024-11-06T04:40:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.03 , 0.09 2024-11-06T04:40:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868004.45 epoch sec; seqNo:711; 4061637.31 north; 594266.39 east; 36.70 lat; -121.94 lon; 45.68 depth; utmZone: 10 2024-11-06T04:40:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868002.43 2024-11-06T04:40:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:04Z- waiting for 1730868006.43, time is 1730868004.45 2024-11-06T04:40:04Z- AHRS_M2 msg: 1730868004.76 epoch sec; seqNo:658 2024-11-06T04:40:04Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.42 psi 2024-11-06T04:40:04Z- handleDepth()- Depth_Keller msg: 1730868004.77 epoch sec; seqNo:712; depth 45.74 2024-11-06T04:40:04Z- handleDVL() - DVL msg epochMillisec: 1730868004.77 2024-11-06T04:40:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:04Z- handleDvl() - RDI_Pathfinder msg: 1730868004.77 sec; seqNo:245; #beams:0 2024-11-06T04:40:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.03 , 0.09 2024-11-06T04:40:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868004.78 epoch sec; seqNo:712; 4061637.69 north; 594266.39 east; 36.70 lat; -121.94 lon; 45.74 depth; utmZone: 10 2024-11-06T04:40:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868002.43 2024-11-06T04:40:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:04Z- waiting for 1730868006.43, time is 1730868004.78 2024-11-06T04:40:05Z- AHRS_M2 msg: 1730868005.16 epoch sec; seqNo:659 2024-11-06T04:40:05Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.43 psi 2024-11-06T04:40:05Z- handleDepth()- Depth_Keller msg: 1730868005.17 epoch sec; seqNo:713; depth 45.80 2024-11-06T04:40:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868005.20 epoch sec; seqNo:713; 4061637.69 north; 594266.39 east; 36.70 lat; -121.94 lon; 45.80 depth; utmZone: 10 2024-11-06T04:40:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868002.43 2024-11-06T04:40:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:05Z- waiting for 1730868006.43, time is 1730868005.20 2024-11-06T04:40:05Z- AHRS_M2 msg: 1730868005.57 epoch sec; seqNo:660 2024-11-06T04:40:05Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.43 psi 2024-11-06T04:40:05Z- handleDepth()- Depth_Keller msg: 1730868005.58 epoch sec; seqNo:714; depth 45.82 2024-11-06T04:40:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868005.62 epoch sec; seqNo:714; 4061638.05 north; 594265.16 east; 36.70 lat; -121.94 lon; 45.82 depth; utmZone: 10 2024-11-06T04:40:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868002.43 2024-11-06T04:40:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:05Z- waiting for 1730868006.43, time is 1730868005.62 2024-11-06T04:40:05Z- AHRS_M2 msg: 1730868005.97 epoch sec; seqNo:661 2024-11-06T04:40:05Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.43 psi 2024-11-06T04:40:05Z- handleDepth()- Depth_Keller msg: 1730868005.98 epoch sec; seqNo:715; depth 45.88 2024-11-06T04:40:05Z- handleDVL() - DVL msg epochMillisec: 1730868005.98 2024-11-06T04:40:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:05Z- handleDvl() - RDI_Pathfinder msg: 1730868005.98 sec; seqNo:246; #beams:0 2024-11-06T04:40:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.03 , 0.09 2024-11-06T04:40:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868005.99 epoch sec; seqNo:715; 4061638.43 north; 594265.16 east; 36.70 lat; -121.94 lon; 45.88 depth; utmZone: 10 2024-11-06T04:40:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868002.43 2024-11-06T04:40:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:06Z- waiting for 1730868006.43, time is 1730868005.99 2024-11-06T04:40:06Z- handleDepth()- Depth_Keller msg: 1730868006.44 epoch sec; seqNo:716; depth 45.92 2024-11-06T04:40:06Z- handleDVL() - DVL msg epochMillisec: 1730868006.47 2024-11-06T04:40:06Z- handleDvl() - RDI_Pathfinder msg: 1730868006.47 sec; seqNo:247; #beams:4 2024-11-06T04:40:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 52.17 , 44.87 , 44.87 , 53.05 2024-11-06T04:40:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , 0.06 2024-11-06T04:40:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868006.48 epoch sec; seqNo:716; 4061638.43 north; 594265.16 east; 36.70 lat; -121.94 lon; 45.92 depth; utmZone: 10 2024-11-06T04:40:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868002.43 2024-11-06T04:40:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:06Z- waiting for 1730868006.43, time is 1730868006.48 2024-11-06T04:40:06Z- LcmTrn::updateTrn() >>>> pose time 1730868006.48, meas time 1730868006.47 <<<< 2024-11-06T04:40:06Z- LcmTrn::updateTrn() equating pose 1730868006.47 and meas times 1730868006.47 2024-11-06T04:40:06Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868006.47 2024-11-06T04:40:06Z- TerrainNav::motionUpdate() - currEstimate = (4061638, 594265.16, 45.92) 2024-11-06T04:40:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:40:06Z- TerrainNav:: SubcloudNIS is 0.12, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:40:06Z- TNavPF::Projecting Measurements 2024-11-06T04:40:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:40:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:40:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:40:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:40:06Z- TNavFilter::4 good beams. 2024-11-06T04:40:06Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:40:06Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:40:06Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:40:06Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:40:06Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:40:06Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:40:06Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:40:06Z- TerrainMapOctree: (vehN, vehE ) = (4061638.43, 594265.16). 2024-11-06T04:40:06Z- TerrainMapOctree: Min Distance = 112.59. 2024-11-06T04:40:06Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:40:06Z- Forcing Subcloud Comparison 2024-11-06T04:40:06Z- Weighting particles with subcloud comparison 2024-11-06T04:40:06Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:06Z- meanExpectedMeasDiff: 0.833743 delta_rms_squared: 1.183325 alpha: 0.983379 2024-11-06T04:40:06Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:06Z- meanExpectedMeasDiff: 0.028303 delta_rms_squared: 0.900687 alpha: 0.978277 2024-11-06T04:40:06Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:06Z- meanExpectedMeasDiff: 0.469488 delta_rms_squared: 0.840372 alpha: 0.976754 2024-11-06T04:40:06Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:06Z- meanExpectedMeasDiff: -0.415675 delta_rms_squared: 1.284572 alpha: 0.984669 2024-11-06T04:40:06Z- Subcloud NIS: 0.115300 2024-11-06T04:40:06Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:40:06Z- TNavPF:: sumWeights = 0.696338 2024-11-06T04:40:06Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:40:06Z- TNavPF::First Term of Map Covariance Matrix is 0.51 2024-11-06T04:40:06Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T04:40:06Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868006.47 sec, ping # 0. 2024-11-06T04:40:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:40:06Z- variances([0], [2], and psi_berg) = 0.110989 0.105969 0.000000 2024-11-06T04:40:06Z- publishEstimatesDecoder() - at 1730868006.47 sec 2024-11-06T04:40:06Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:40:06Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061581.92 594269.82 52.97 2024-11-06T04:40:06Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061582.40 594269.26 53.07 lat/lon: 36.70 -121.94 2024-11-06T04:40:06Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.11 0.11 0.43 0.00 2024-11-06T04:40:06Z- AHRS_M2 msg: 1730868006.79 epoch sec; seqNo:662 2024-11-06T04:40:06Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 5.43 psi 2024-11-06T04:40:06Z- handleDepth()- Depth_Keller msg: 1730868006.80 epoch sec; seqNo:717; depth 45.99 2024-11-06T04:40:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868006.81 epoch sec; seqNo:717; 4061638.81 north; 594265.15 east; 36.70 lat; -121.94 lon; 45.99 depth; utmZone: 10 2024-11-06T04:40:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868006.47 2024-11-06T04:40:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:06Z- waiting for 1730868010.47, time is 1730868006.81 2024-11-06T04:40:07Z- AHRS_M2 msg: 1730868007.19 epoch sec; seqNo:663 2024-11-06T04:40:07Z- AHRS_M2 msg: 0.13 phi; -0.18 theta; 5.44 psi 2024-11-06T04:40:07Z- handleDepth()- Depth_Keller msg: 1730868007.20 epoch sec; seqNo:718; depth 46.03 2024-11-06T04:40:07Z- handleDVL() - DVL msg epochMillisec: 1730868007.20 2024-11-06T04:40:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:07Z- handleDvl() - RDI_Pathfinder msg: 1730868007.20 sec; seqNo:248; #beams:0 2024-11-06T04:40:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , 0.06 2024-11-06T04:40:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868007.21 epoch sec; seqNo:718; 4061639.18 north; 594263.93 east; 36.70 lat; -121.94 lon; 46.03 depth; utmZone: 10 2024-11-06T04:40:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868006.47 2024-11-06T04:40:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:07Z- waiting for 1730868010.47, time is 1730868007.21 2024-11-06T04:40:07Z- handleDepth()- Depth_Keller msg: 1730868007.60 epoch sec; seqNo:719; depth 46.09 2024-11-06T04:40:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868007.62 epoch sec; seqNo:719; 4061639.18 north; 594263.93 east; 36.70 lat; -121.94 lon; 46.09 depth; utmZone: 10 2024-11-06T04:40:07Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:40:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868006.47 2024-11-06T04:40:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:07Z- waiting for 1730868010.47, time is 1730868007.62 2024-11-06T04:40:07Z- AHRS_M2 msg: 1730868007.99 epoch sec; seqNo:664 2024-11-06T04:40:07Z- AHRS_M2 msg: 0.13 phi; -0.18 theta; 5.44 psi 2024-11-06T04:40:08Z- handleDepth()- Depth_Keller msg: 1730868008.00 epoch sec; seqNo:720; depth 46.14 2024-11-06T04:40:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868008.02 epoch sec; seqNo:720; 4061639.55 north; 594263.92 east; 36.70 lat; -121.94 lon; 46.14 depth; utmZone: 10 2024-11-06T04:40:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868006.47 2024-11-06T04:40:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:08Z- waiting for 1730868010.47, time is 1730868008.02 2024-11-06T04:40:08Z- AHRS_M2 msg: 1730868008.43 epoch sec; seqNo:665 2024-11-06T04:40:08Z- AHRS_M2 msg: 0.13 phi; -0.18 theta; 5.43 psi 2024-11-06T04:40:08Z- handleDepth()- Depth_Keller msg: 1730868008.44 epoch sec; seqNo:721; depth 46.18 2024-11-06T04:40:08Z- handleDVL() - DVL msg epochMillisec: 1730868008.46 2024-11-06T04:40:08Z- handleDvl() - RDI_Pathfinder msg: 1730868008.46 sec; seqNo:249; #beams:4 2024-11-06T04:40:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 52.39 , 44.65 , 44.43 , 53.27 2024-11-06T04:40:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , 0.06 2024-11-06T04:40:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868008.48 epoch sec; seqNo:721; 4061639.93 north; 594263.92 east; 36.70 lat; -121.94 lon; 46.18 depth; utmZone: 10 2024-11-06T04:40:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868006.47 2024-11-06T04:40:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:08Z- waiting for 1730868010.47, time is 1730868008.48 2024-11-06T04:40:08Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:40:08Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:40:08Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:40:08Z- AHRS_M2 msg: 1730868008.79 epoch sec; seqNo:666 2024-11-06T04:40:08Z- AHRS_M2 msg: 0.13 phi; -0.18 theta; 5.43 psi 2024-11-06T04:40:08Z- handleDepth()- Depth_Keller msg: 1730868008.82 epoch sec; seqNo:722; depth 46.24 2024-11-06T04:40:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868008.84 epoch sec; seqNo:722; 4061639.93 north; 594263.92 east; 36.70 lat; -121.94 lon; 46.24 depth; utmZone: 10 2024-11-06T04:40:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868006.47 2024-11-06T04:40:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:08Z- waiting for 1730868010.47, time is 1730868008.84 2024-11-06T04:40:09Z- AHRS_M2 msg: 1730868009.21 epoch sec; seqNo:667 2024-11-06T04:40:09Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.43 psi 2024-11-06T04:40:09Z- handleDepth()- Depth_Keller msg: 1730868009.21 epoch sec; seqNo:723; depth 46.28 2024-11-06T04:40:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868009.23 epoch sec; seqNo:723; 4061640.30 north; 594262.69 east; 36.70 lat; -121.94 lon; 46.28 depth; utmZone: 10 2024-11-06T04:40:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868006.47 2024-11-06T04:40:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:09Z- waiting for 1730868010.47, time is 1730868009.23 2024-11-06T04:40:09Z- AHRS_M2 msg: 1730868009.62 epoch sec; seqNo:668 2024-11-06T04:40:09Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.43 psi 2024-11-06T04:40:09Z- handleDepth()- Depth_Keller msg: 1730868009.62 epoch sec; seqNo:724; depth 46.32 2024-11-06T04:40:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868009.63 epoch sec; seqNo:724; 4061640.68 north; 594262.69 east; 36.70 lat; -121.94 lon; 46.32 depth; utmZone: 10 2024-11-06T04:40:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868006.47 2024-11-06T04:40:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:09Z- waiting for 1730868010.47, time is 1730868009.63 2024-11-06T04:40:10Z- AHRS_M2 msg: 1730868010.01 epoch sec; seqNo:669 2024-11-06T04:40:10Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.43 psi 2024-11-06T04:40:10Z- handleDepth()- Depth_Keller msg: 1730868010.02 epoch sec; seqNo:725; depth 46.38 2024-11-06T04:40:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868010.04 epoch sec; seqNo:725; 4061640.68 north; 594262.69 east; 36.70 lat; -121.94 lon; 46.38 depth; utmZone: 10 2024-11-06T04:40:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868006.47 2024-11-06T04:40:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:10Z- waiting for 1730868010.47, time is 1730868010.04 2024-11-06T04:40:10Z- AHRS_M2 msg: 1730868010.42 epoch sec; seqNo:670 2024-11-06T04:40:10Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.43 psi 2024-11-06T04:40:10Z- handleDepth()- Depth_Keller msg: 1730868010.43 epoch sec; seqNo:726; depth 46.43 2024-11-06T04:40:10Z- handleDVL() - DVL msg epochMillisec: 1730868010.45 2024-11-06T04:40:10Z- handleDvl() - RDI_Pathfinder msg: 1730868010.45 sec; seqNo:250; #beams:4 2024-11-06T04:40:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 52.17 , 44.43 , 44.21 , 52.83 2024-11-06T04:40:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , 0.06 2024-11-06T04:40:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868010.49 epoch sec; seqNo:726; 4061641.06 north; 594262.69 east; 36.70 lat; -121.94 lon; 46.43 depth; utmZone: 10 2024-11-06T04:40:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868006.47 2024-11-06T04:40:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:10Z- waiting for 1730868010.47, time is 1730868010.49 2024-11-06T04:40:10Z- LcmTrn::updateTrn() >>>> pose time 1730868010.49, meas time 1730868010.45 <<<< 2024-11-06T04:40:10Z- LcmTrn::updateTrn() equating pose 1730868010.45 and meas times 1730868010.45 2024-11-06T04:40:10Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868010.45 2024-11-06T04:40:10Z- TerrainNav::motionUpdate() - currEstimate = (4061641, 594262.69, 46.43) 2024-11-06T04:40:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:40:10Z- TerrainNav:: SubcloudNIS is 0.12, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:40:10Z- TNavPF::Projecting Measurements 2024-11-06T04:40:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:40:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:40:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:40:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:40:10Z- TNavFilter::4 good beams. 2024-11-06T04:40:10Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:40:10Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:40:10Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:40:10Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:40:10Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:40:10Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:40:10Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:40:10Z- TerrainMapOctree: (vehN, vehE ) = (4061641.06, 594262.69). 2024-11-06T04:40:10Z- TerrainMapOctree: Min Distance = 109.68. 2024-11-06T04:40:10Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:40:10Z- Forcing Subcloud Comparison 2024-11-06T04:40:10Z- Weighting particles with subcloud comparison 2024-11-06T04:40:10Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:10Z- meanExpectedMeasDiff: 0.939231 delta_rms_squared: 1.102117 alpha: 0.982177 2024-11-06T04:40:10Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:10Z- meanExpectedMeasDiff: 0.069165 delta_rms_squared: 0.822000 alpha: 0.976247 2024-11-06T04:40:10Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:10Z- meanExpectedMeasDiff: 0.426356 delta_rms_squared: 0.761509 alpha: 0.974408 2024-11-06T04:40:10Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:10Z- meanExpectedMeasDiff: -0.778559 delta_rms_squared: 1.205026 alpha: 0.983674 2024-11-06T04:40:10Z- Subcloud NIS: 0.115328 2024-11-06T04:40:10Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:40:10Z- TNavPF:: sumWeights = 0.671955 2024-11-06T04:40:10Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:40:10Z- TNavPF::First Term of Map Covariance Matrix is 0.49 2024-11-06T04:40:10Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T04:40:10Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868010.45 sec, ping # 0. 2024-11-06T04:40:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:40:10Z- variances([0], [2], and psi_berg) = 0.113483 0.108726 0.000000 2024-11-06T04:40:10Z- publishEstimatesDecoder() - at 1730868010.45 sec 2024-11-06T04:40:10Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:40:10Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061585.47 594267.05 53.44 2024-11-06T04:40:10Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061585.03 594266.79 53.51 lat/lon: 36.70 -121.94 2024-11-06T04:40:10Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.11 0.11 0.40 0.00 2024-11-06T04:40:10Z- AHRS_M2 msg: 1730868010.83 epoch sec; seqNo:671 2024-11-06T04:40:10Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.43 psi 2024-11-06T04:40:10Z- handleDepth()- Depth_Keller msg: 1730868010.85 epoch sec; seqNo:727; depth 46.46 2024-11-06T04:40:10Z- handleDVL() - DVL msg epochMillisec: 1730868010.85 2024-11-06T04:40:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:10Z- handleDvl() - RDI_Pathfinder msg: 1730868010.85 sec; seqNo:251; #beams:0 2024-11-06T04:40:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , 0.06 2024-11-06T04:40:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868010.88 epoch sec; seqNo:727; 4061641.04 north; 594261.47 east; 36.70 lat; -121.94 lon; 46.46 depth; utmZone: 10 2024-11-06T04:40:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868010.45 2024-11-06T04:40:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:10Z- waiting for 1730868014.45, time is 1730868010.88 2024-11-06T04:40:11Z- AHRS_M2 msg: 1730868011.22 epoch sec; seqNo:672 2024-11-06T04:40:11Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.43 psi 2024-11-06T04:40:11Z- handleDepth()- Depth_Keller msg: 1730868011.24 epoch sec; seqNo:728; depth 46.51 2024-11-06T04:40:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868011.26 epoch sec; seqNo:728; 4061641.42 north; 594261.46 east; 36.70 lat; -121.94 lon; 46.51 depth; utmZone: 10 2024-11-06T04:40:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868010.45 2024-11-06T04:40:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:11Z- waiting for 1730868014.45, time is 1730868011.26 2024-11-06T04:40:11Z- AHRS_M2 msg: 1730868011.63 epoch sec; seqNo:673 2024-11-06T04:40:11Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.43 psi 2024-11-06T04:40:11Z- handleDepth()- Depth_Keller msg: 1730868011.64 epoch sec; seqNo:729; depth 46.56 2024-11-06T04:40:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868011.65 epoch sec; seqNo:729; 4061641.80 north; 594261.46 east; 36.70 lat; -121.94 lon; 46.56 depth; utmZone: 10 2024-11-06T04:40:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868010.45 2024-11-06T04:40:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:11Z- waiting for 1730868014.45, time is 1730868011.65 2024-11-06T04:40:12Z- AHRS_M2 msg: 1730868012.03 epoch sec; seqNo:674 2024-11-06T04:40:12Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.43 psi 2024-11-06T04:40:12Z- handleDepth()- Depth_Keller msg: 1730868012.04 epoch sec; seqNo:730; depth 46.60 2024-11-06T04:40:12Z- handleDVL() - DVL msg epochMillisec: 1730868012.04 2024-11-06T04:40:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:12Z- handleDvl() - RDI_Pathfinder msg: 1730868012.04 sec; seqNo:252; #beams:0 2024-11-06T04:40:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , 0.06 2024-11-06T04:40:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868012.05 epoch sec; seqNo:730; 4061641.80 north; 594261.46 east; 36.70 lat; -121.94 lon; 46.60 depth; utmZone: 10 2024-11-06T04:40:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868010.45 2024-11-06T04:40:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:12Z- waiting for 1730868014.45, time is 1730868012.05 2024-11-06T04:40:12Z- AHRS_M2 msg: 1730868012.43 epoch sec; seqNo:675 2024-11-06T04:40:12Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.43 psi 2024-11-06T04:40:12Z- handleDepth()- Depth_Keller msg: 1730868012.45 epoch sec; seqNo:731; depth 46.65 2024-11-06T04:40:12Z- handleDVL() - DVL msg epochMillisec: 1730868012.47 2024-11-06T04:40:12Z- handleDvl() - RDI_Pathfinder msg: 1730868012.47 sec; seqNo:253; #beams:4 2024-11-06T04:40:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 51.50 , 44.43 , 43.76 , 53.05 2024-11-06T04:40:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.03 2024-11-06T04:40:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868012.49 epoch sec; seqNo:731; 4061642.17 north; 594260.23 east; 36.70 lat; -121.94 lon; 46.65 depth; utmZone: 10 2024-11-06T04:40:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868010.45 2024-11-06T04:40:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:12Z- waiting for 1730868014.45, time is 1730868012.49 2024-11-06T04:40:12Z- AHRS_M2 msg: 1730868012.84 epoch sec; seqNo:676 2024-11-06T04:40:12Z- AHRS_M2 msg: 0.13 phi; -0.20 theta; 5.44 psi 2024-11-06T04:40:12Z- handleDepth()- Depth_Keller msg: 1730868012.85 epoch sec; seqNo:732; depth 46.69 2024-11-06T04:40:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868012.87 epoch sec; seqNo:732; 4061642.17 north; 594260.23 east; 36.70 lat; -121.94 lon; 46.69 depth; utmZone: 10 2024-11-06T04:40:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868010.45 2024-11-06T04:40:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:13Z- waiting for 1730868014.45, time is 1730868012.87 2024-11-06T04:40:13Z- AHRS_M2 msg: 1730868013.24 epoch sec; seqNo:677 2024-11-06T04:40:13Z- AHRS_M2 msg: 0.13 phi; -0.20 theta; 5.44 psi 2024-11-06T04:40:13Z- handleDepth()- Depth_Keller msg: 1730868013.25 epoch sec; seqNo:733; depth 46.74 2024-11-06T04:40:13Z- handleDVL() - DVL msg epochMillisec: 1730868013.25 2024-11-06T04:40:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:13Z- handleDvl() - RDI_Pathfinder msg: 1730868013.25 sec; seqNo:254; #beams:0 2024-11-06T04:40:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.03 2024-11-06T04:40:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868013.27 epoch sec; seqNo:733; 4061642.55 north; 594260.23 east; 36.70 lat; -121.94 lon; 46.74 depth; utmZone: 10 2024-11-06T04:40:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868010.45 2024-11-06T04:40:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:13Z- waiting for 1730868014.45, time is 1730868013.27 2024-11-06T04:40:13Z- AHRS_M2 msg: 1730868013.65 epoch sec; seqNo:678 2024-11-06T04:40:13Z- AHRS_M2 msg: 0.13 phi; -0.20 theta; 5.44 psi 2024-11-06T04:40:13Z- handleDepth()- Depth_Keller msg: 1730868013.65 epoch sec; seqNo:734; depth 46.78 2024-11-06T04:40:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868013.66 epoch sec; seqNo:734; 4061642.92 north; 594260.23 east; 36.70 lat; -121.94 lon; 46.78 depth; utmZone: 10 2024-11-06T04:40:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868010.45 2024-11-06T04:40:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:13Z- waiting for 1730868014.45, time is 1730868013.66 2024-11-06T04:40:14Z- AHRS_M2 msg: 1730868014.05 epoch sec; seqNo:679 2024-11-06T04:40:14Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.44 psi 2024-11-06T04:40:14Z- handleDepth()- Depth_Keller msg: 1730868014.06 epoch sec; seqNo:735; depth 46.82 2024-11-06T04:40:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868014.07 epoch sec; seqNo:735; 4061642.92 north; 594260.23 east; 36.70 lat; -121.94 lon; 46.82 depth; utmZone: 10 2024-11-06T04:40:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868010.45 2024-11-06T04:40:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:14Z- waiting for 1730868014.45, time is 1730868014.07 2024-11-06T04:40:14Z- AHRS_M2 msg: 1730868014.45 epoch sec; seqNo:680 2024-11-06T04:40:14Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.44 psi 2024-11-06T04:40:14Z- handleDepth()- Depth_Keller msg: 1730868014.46 epoch sec; seqNo:736; depth 46.88 2024-11-06T04:40:14Z- handleDVL() - DVL msg epochMillisec: 1730868014.49 2024-11-06T04:40:14Z- handleDvl() - RDI_Pathfinder msg: 1730868014.49 sec; seqNo:255; #beams:4 2024-11-06T04:40:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 51.72 , 43.99 , 43.32 , 53.05 2024-11-06T04:40:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , 0.00 2024-11-06T04:40:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868014.51 epoch sec; seqNo:736; 4061643.29 north; 594259.00 east; 36.70 lat; -121.94 lon; 46.88 depth; utmZone: 10 2024-11-06T04:40:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868010.45 2024-11-06T04:40:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:14Z- waiting for 1730868014.45, time is 1730868014.51 2024-11-06T04:40:14Z- LcmTrn::updateTrn() >>>> pose time 1730868014.51, meas time 1730868014.49 <<<< 2024-11-06T04:40:14Z- LcmTrn::updateTrn() equating pose 1730868014.49 and meas times 1730868014.49 2024-11-06T04:40:14Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868014.49 2024-11-06T04:40:14Z- TerrainNav::motionUpdate() - currEstimate = (4061643, 594259.00, 46.88) 2024-11-06T04:40:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:40:14Z- TerrainNav:: SubcloudNIS is 0.12, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:40:14Z- TNavPF::Projecting Measurements 2024-11-06T04:40:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:40:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:40:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:40:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:40:14Z- TNavFilter::4 good beams. 2024-11-06T04:40:14Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:40:14Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:40:14Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:40:14Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:40:14Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:40:14Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:40:14Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:40:14Z- TerrainMapOctree: (vehN, vehE ) = (4061643.29, 594259.00). 2024-11-06T04:40:14Z- TerrainMapOctree: Min Distance = 107.09. 2024-11-06T04:40:14Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:40:14Z- Forcing Subcloud Comparison 2024-11-06T04:40:14Z- Weighting particles with subcloud comparison 2024-11-06T04:40:14Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:14Z- meanExpectedMeasDiff: -0.276244 delta_rms_squared: 1.047367 alpha: 0.981262 2024-11-06T04:40:14Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:14Z- meanExpectedMeasDiff: -0.026301 delta_rms_squared: 0.785065 alpha: 0.975157 2024-11-06T04:40:14Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:14Z- meanExpectedMeasDiff: 1.145444 delta_rms_squared: 0.717424 alpha: 0.972879 2024-11-06T04:40:14Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:14Z- meanExpectedMeasDiff: -0.353270 delta_rms_squared: 1.160462 alpha: 0.983057 2024-11-06T04:40:14Z- Subcloud NIS: 0.113866 2024-11-06T04:40:14Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:40:14Z- TNavPF:: sumWeights = 0.667121 2024-11-06T04:40:14Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:40:14Z- TNavPF::First Term of Map Covariance Matrix is 0.43 2024-11-06T04:40:14Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T04:40:14Z- TerrainNav::Resampling particle filter... 2024-11-06T04:40:14Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868014.49 sec, ping # 0. 2024-11-06T04:40:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:40:14Z- variances([0], [2], and psi_berg) = 0.115819 0.112056 0.000000 2024-11-06T04:40:14Z- publishEstimatesDecoder() - at 1730868014.49 sec 2024-11-06T04:40:14Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:40:14Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061587.15 594262.96 53.63 2024-11-06T04:40:14Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061587.26 594263.10 53.93 lat/lon: 36.70 -121.94 2024-11-06T04:40:14Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.12 0.11 0.38 0.00 2024-11-06T04:40:14Z- handleDepth()- Depth_Keller msg: 1730868014.90 epoch sec; seqNo:737; depth 46.92 2024-11-06T04:40:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868014.92 epoch sec; seqNo:737; 4061643.67 north; 594259.00 east; 36.70 lat; -121.94 lon; 46.92 depth; utmZone: 10 2024-11-06T04:40:15Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:40:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868014.49 2024-11-06T04:40:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:15Z- waiting for 1730868018.49, time is 1730868014.92 2024-11-06T04:40:15Z- AHRS_M2 msg: 1730868015.27 epoch sec; seqNo:681 2024-11-06T04:40:15Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.44 psi 2024-11-06T04:40:15Z- handleDepth()- Depth_Keller msg: 1730868015.30 epoch sec; seqNo:738; depth 46.99 2024-11-06T04:40:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868015.32 epoch sec; seqNo:738; 4061643.67 north; 594259.00 east; 36.70 lat; -121.94 lon; 46.99 depth; utmZone: 10 2024-11-06T04:40:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868014.49 2024-11-06T04:40:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:15Z- waiting for 1730868018.49, time is 1730868015.32 2024-11-06T04:40:15Z- AHRS_M2 msg: 1730868015.67 epoch sec; seqNo:682 2024-11-06T04:40:15Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.44 psi 2024-11-06T04:40:15Z- handleDepth()- Depth_Keller msg: 1730868015.68 epoch sec; seqNo:739; depth 47.02 2024-11-06T04:40:15Z- handleDVL() - DVL msg epochMillisec: 1730868015.68 2024-11-06T04:40:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:15Z- handleDvl() - RDI_Pathfinder msg: 1730868015.68 sec; seqNo:256; #beams:0 2024-11-06T04:40:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , 0.00 2024-11-06T04:40:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868015.69 epoch sec; seqNo:739; 4061644.05 north; 594258.99 east; 36.70 lat; -121.94 lon; 47.02 depth; utmZone: 10 2024-11-06T04:40:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868014.49 2024-11-06T04:40:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:15Z- waiting for 1730868018.49, time is 1730868015.69 2024-11-06T04:40:16Z- AHRS_M2 msg: 1730868016.07 epoch sec; seqNo:683 2024-11-06T04:40:16Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.44 psi 2024-11-06T04:40:16Z- handleDepth()- Depth_Keller msg: 1730868016.08 epoch sec; seqNo:740; depth 47.06 2024-11-06T04:40:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868016.10 epoch sec; seqNo:740; 4061644.03 north; 594257.77 east; 36.70 lat; -121.94 lon; 47.06 depth; utmZone: 10 2024-11-06T04:40:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868014.49 2024-11-06T04:40:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:16Z- waiting for 1730868018.49, time is 1730868016.10 2024-11-06T04:40:16Z- AHRS_M2 msg: 1730868016.48 epoch sec; seqNo:684 2024-11-06T04:40:16Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.44 psi 2024-11-06T04:40:16Z- handleDepth()- Depth_Keller msg: 1730868016.48 epoch sec; seqNo:741; depth 47.11 2024-11-06T04:40:16Z- handleDVL() - DVL msg epochMillisec: 1730868016.52 2024-11-06T04:40:16Z- handleDvl() - RDI_Pathfinder msg: 1730868016.52 sec; seqNo:257; #beams:4 2024-11-06T04:40:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.84 , 43.76 , 43.10 , 52.61 2024-11-06T04:40:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.04 , 0.02 2024-11-06T04:40:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868016.53 epoch sec; seqNo:741; 4061644.41 north; 594257.77 east; 36.70 lat; -121.94 lon; 47.11 depth; utmZone: 10 2024-11-06T04:40:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868014.49 2024-11-06T04:40:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:16Z- waiting for 1730868018.49, time is 1730868016.53 2024-11-06T04:40:16Z- AHRS_M2 msg: 1730868016.88 epoch sec; seqNo:685 2024-11-06T04:40:16Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.44 psi 2024-11-06T04:40:16Z- handleDepth()- Depth_Keller msg: 1730868016.88 epoch sec; seqNo:742; depth 47.17 2024-11-06T04:40:16Z- handleDVL() - DVL msg epochMillisec: 1730868016.89 2024-11-06T04:40:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:16Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:16Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:16Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:16Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:16Z- handleDvl() - RDI_Pathfinder msg: 1730868016.89 sec; seqNo:258; #beams:0 2024-11-06T04:40:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.04 , 0.02 2024-11-06T04:40:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868016.90 epoch sec; seqNo:742; 4061644.79 north; 594257.76 east; 36.70 lat; -121.94 lon; 47.17 depth; utmZone: 10 2024-11-06T04:40:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868014.49 2024-11-06T04:40:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:17Z- waiting for 1730868018.49, time is 1730868016.90 2024-11-06T04:40:17Z- AHRS_M2 msg: 1730868017.29 epoch sec; seqNo:686 2024-11-06T04:40:17Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.44 psi 2024-11-06T04:40:17Z- handleDepth()- Depth_Keller msg: 1730868017.30 epoch sec; seqNo:743; depth 47.23 2024-11-06T04:40:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868017.32 epoch sec; seqNo:743; 4061644.79 north; 594257.76 east; 36.70 lat; -121.94 lon; 47.23 depth; utmZone: 10 2024-11-06T04:40:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868014.49 2024-11-06T04:40:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:17Z- waiting for 1730868018.49, time is 1730868017.32 2024-11-06T04:40:17Z- AHRS_M2 msg: 1730868017.69 epoch sec; seqNo:687 2024-11-06T04:40:17Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.44 psi 2024-11-06T04:40:17Z- handleDepth()- Depth_Keller msg: 1730868017.70 epoch sec; seqNo:744; depth 47.28 2024-11-06T04:40:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868017.72 epoch sec; seqNo:744; 4061645.16 north; 594256.54 east; 36.70 lat; -121.94 lon; 47.28 depth; utmZone: 10 2024-11-06T04:40:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868014.49 2024-11-06T04:40:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:17Z- waiting for 1730868018.49, time is 1730868017.72 2024-11-06T04:40:18Z- AHRS_M2 msg: 1730868018.09 epoch sec; seqNo:688 2024-11-06T04:40:18Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.43 psi 2024-11-06T04:40:18Z- handleDepth()- Depth_Keller msg: 1730868018.10 epoch sec; seqNo:745; depth 47.33 2024-11-06T04:40:18Z- handleDVL() - DVL msg epochMillisec: 1730868018.10 2024-11-06T04:40:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:18Z- handleDvl() - RDI_Pathfinder msg: 1730868018.10 sec; seqNo:259; #beams:0 2024-11-06T04:40:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.04 , 0.02 2024-11-06T04:40:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868018.11 epoch sec; seqNo:745; 4061645.54 north; 594256.53 east; 36.70 lat; -121.94 lon; 47.33 depth; utmZone: 10 2024-11-06T04:40:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868014.49 2024-11-06T04:40:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:18Z- waiting for 1730868018.49, time is 1730868018.11 2024-11-06T04:40:18Z- AHRS_M2 msg: 1730868018.50 epoch sec; seqNo:689 2024-11-06T04:40:18Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.43 psi 2024-11-06T04:40:18Z- handleDepth()- Depth_Keller msg: 1730868018.51 epoch sec; seqNo:746; depth 47.40 2024-11-06T04:40:18Z- handleDVL() - DVL msg epochMillisec: 1730868018.53 2024-11-06T04:40:18Z- handleDvl() - RDI_Pathfinder msg: 1730868018.53 sec; seqNo:260; #beams:4 2024-11-06T04:40:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.62 , 43.54 , 42.88 , 52.16 2024-11-06T04:40:18Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.03 , 0.07 2024-11-06T04:40:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868018.59 epoch sec; seqNo:746; 4061645.54 north; 594256.53 east; 36.70 lat; -121.94 lon; 47.40 depth; utmZone: 10 2024-11-06T04:40:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868014.49 2024-11-06T04:40:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:18Z- waiting for 1730868018.49, time is 1730868018.59 2024-11-06T04:40:18Z- LcmTrn::updateTrn() >>>> pose time 1730868018.59, meas time 1730868018.53 <<<< 2024-11-06T04:40:18Z- LcmTrn::updateTrn() equating pose 1730868018.53 and meas times 1730868018.53 2024-11-06T04:40:18Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868018.53 2024-11-06T04:40:18Z- TerrainNav::motionUpdate() - currEstimate = (4061646, 594256.53, 47.40) 2024-11-06T04:40:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:40:18Z- TerrainNav:: SubcloudNIS is 0.11, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:40:18Z- TNavPF::Projecting Measurements 2024-11-06T04:40:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:40:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:40:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:40:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:40:18Z- TNavFilter::4 good beams. 2024-11-06T04:40:18Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:40:18Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:40:18Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:40:18Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:40:18Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:40:18Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:40:18Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:40:18Z- TerrainMapOctree: (vehN, vehE ) = (4061645.54, 594256.53). 2024-11-06T04:40:18Z- TerrainMapOctree: Min Distance = 104.67. 2024-11-06T04:40:18Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:40:18Z- Forcing Subcloud Comparison 2024-11-06T04:40:18Z- Weighting particles with subcloud comparison 2024-11-06T04:40:18Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:18Z- meanExpectedMeasDiff: -0.473240 delta_rms_squared: 1.003751 alpha: 0.980464 2024-11-06T04:40:18Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:18Z- meanExpectedMeasDiff: -0.136951 delta_rms_squared: 0.769250 alpha: 0.974659 2024-11-06T04:40:18Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:18Z- meanExpectedMeasDiff: 0.722277 delta_rms_squared: 1.373812 alpha: 0.985651 2024-11-06T04:40:18Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:18Z- meanExpectedMeasDiff: -0.664061 delta_rms_squared: 1.129835 alpha: 0.982606 2024-11-06T04:40:18Z- Subcloud NIS: 0.115615 2024-11-06T04:40:18Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:40:18Z- TNavPF:: sumWeights = 0.659490 2024-11-06T04:40:18Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:40:18Z- TNavPF::First Term of Map Covariance Matrix is 0.40 2024-11-06T04:40:18Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T04:40:18Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868018.53 sec, ping # 0. 2024-11-06T04:40:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:40:18Z- variances([0], [2], and psi_berg) = 0.121911 0.117207 0.000000 2024-11-06T04:40:18Z- publishEstimatesDecoder() - at 1730868018.53 sec 2024-11-06T04:40:18Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:40:18Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061589.95 594260.58 54.66 2024-11-06T04:40:18Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061589.52 594260.62 54.46 lat/lon: 36.70 -121.94 2024-11-06T04:40:18Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.12 0.12 0.36 0.00 2024-11-06T04:40:18Z- AHRS_M2 msg: 1730868018.90 epoch sec; seqNo:690 2024-11-06T04:40:18Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.43 psi 2024-11-06T04:40:18Z- handleDepth()- Depth_Keller msg: 1730868018.91 epoch sec; seqNo:747; depth 47.46 2024-11-06T04:40:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868018.93 epoch sec; seqNo:747; 4061645.91 north; 594256.53 east; 36.70 lat; -121.94 lon; 47.46 depth; utmZone: 10 2024-11-06T04:40:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868018.53 2024-11-06T04:40:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:19Z- waiting for 1730868022.53, time is 1730868018.93 2024-11-06T04:40:19Z- AHRS_M2 msg: 1730868019.30 epoch sec; seqNo:691 2024-11-06T04:40:19Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.43 psi 2024-11-06T04:40:19Z- handleDepth()- Depth_Keller msg: 1730868019.31 epoch sec; seqNo:748; depth 47.53 2024-11-06T04:40:19Z- handleDVL() - DVL msg epochMillisec: 1730868019.31 2024-11-06T04:40:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:19Z- handleDvl() - RDI_Pathfinder msg: 1730868019.31 sec; seqNo:261; #beams:0 2024-11-06T04:40:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:19Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.03 , 0.07 2024-11-06T04:40:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868019.32 epoch sec; seqNo:748; 4061646.28 north; 594255.31 east; 36.70 lat; -121.94 lon; 47.53 depth; utmZone: 10 2024-11-06T04:40:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868018.53 2024-11-06T04:40:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:19Z- waiting for 1730868022.53, time is 1730868019.32 2024-11-06T04:40:19Z- AHRS_M2 msg: 1730868019.71 epoch sec; seqNo:692 2024-11-06T04:40:19Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.43 psi 2024-11-06T04:40:19Z- handleDepth()- Depth_Keller msg: 1730868019.72 epoch sec; seqNo:749; depth 47.57 2024-11-06T04:40:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868019.73 epoch sec; seqNo:749; 4061646.28 north; 594255.31 east; 36.70 lat; -121.94 lon; 47.57 depth; utmZone: 10 2024-11-06T04:40:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868018.53 2024-11-06T04:40:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:19Z- waiting for 1730868022.53, time is 1730868019.73 2024-11-06T04:40:20Z- AHRS_M2 msg: 1730868020.11 epoch sec; seqNo:693 2024-11-06T04:40:20Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.43 psi 2024-11-06T04:40:20Z- handleDepth()- Depth_Keller msg: 1730868020.12 epoch sec; seqNo:750; depth 47.64 2024-11-06T04:40:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868020.13 epoch sec; seqNo:750; 4061646.66 north; 594255.30 east; 36.70 lat; -121.94 lon; 47.64 depth; utmZone: 10 2024-11-06T04:40:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868018.53 2024-11-06T04:40:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:20Z- waiting for 1730868022.53, time is 1730868020.13 2024-11-06T04:40:20Z- AHRS_M2 msg: 1730868020.52 epoch sec; seqNo:694 2024-11-06T04:40:20Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.43 psi 2024-11-06T04:40:20Z- handleDepth()- Depth_Keller msg: 1730868020.53 epoch sec; seqNo:751; depth 47.70 2024-11-06T04:40:20Z- handleDVL() - DVL msg epochMillisec: 1730868020.56 2024-11-06T04:40:20Z- handleDvl() - RDI_Pathfinder msg: 1730868020.56 sec; seqNo:262; #beams:4 2024-11-06T04:40:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.17 , 43.32 , 42.66 , 51.50 2024-11-06T04:40:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.01 , 0.11 2024-11-06T04:40:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868020.57 epoch sec; seqNo:751; 4061647.04 north; 594255.30 east; 36.70 lat; -121.94 lon; 47.70 depth; utmZone: 10 2024-11-06T04:40:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868018.53 2024-11-06T04:40:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:20Z- waiting for 1730868022.53, time is 1730868020.57 2024-11-06T04:40:20Z- AHRS_M2 msg: 1730868020.92 epoch sec; seqNo:695 2024-11-06T04:40:20Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.43 psi 2024-11-06T04:40:20Z- handleDepth()- Depth_Keller msg: 1730868020.92 epoch sec; seqNo:752; depth 47.75 2024-11-06T04:40:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868020.94 epoch sec; seqNo:752; 4061647.02 north; 594254.08 east; 36.70 lat; -121.94 lon; 47.75 depth; utmZone: 10 2024-11-06T04:40:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868018.53 2024-11-06T04:40:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:21Z- waiting for 1730868022.53, time is 1730868020.94 2024-11-06T04:40:21Z- AHRS_M2 msg: 1730868021.32 epoch sec; seqNo:696 2024-11-06T04:40:21Z- AHRS_M2 msg: 0.11 phi; -0.20 theta; 5.43 psi 2024-11-06T04:40:21Z- handleDepth()- Depth_Keller msg: 1730868021.33 epoch sec; seqNo:753; depth 47.82 2024-11-06T04:40:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868021.34 epoch sec; seqNo:753; 4061647.40 north; 594254.07 east; 36.70 lat; -121.94 lon; 47.82 depth; utmZone: 10 2024-11-06T04:40:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868018.53 2024-11-06T04:40:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:21Z- waiting for 1730868022.53, time is 1730868021.34 2024-11-06T04:40:21Z- AHRS_M2 msg: 1730868021.74 epoch sec; seqNo:697 2024-11-06T04:40:21Z- AHRS_M2 msg: 0.12 phi; -0.20 theta; 5.43 psi 2024-11-06T04:40:21Z- handleDepth()- Depth_Keller msg: 1730868021.75 epoch sec; seqNo:754; depth 47.88 2024-11-06T04:40:21Z- handleDVL() - DVL msg epochMillisec: 1730868021.76 2024-11-06T04:40:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:21Z- handleDvl() - RDI_Pathfinder msg: 1730868021.76 sec; seqNo:263; #beams:0 2024-11-06T04:40:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.01 , 0.11 2024-11-06T04:40:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868021.77 epoch sec; seqNo:754; 4061647.40 north; 594254.07 east; 36.70 lat; -121.94 lon; 47.88 depth; utmZone: 10 2024-11-06T04:40:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868018.53 2024-11-06T04:40:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:21Z- waiting for 1730868022.53, time is 1730868021.77 2024-11-06T04:40:22Z- AHRS_M2 msg: 1730868022.13 epoch sec; seqNo:698 2024-11-06T04:40:22Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.42 psi 2024-11-06T04:40:22Z- handleDepth()- Depth_Keller msg: 1730868022.14 epoch sec; seqNo:755; depth 47.92 2024-11-06T04:40:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868022.15 epoch sec; seqNo:755; 4061647.78 north; 594254.07 east; 36.70 lat; -121.94 lon; 47.92 depth; utmZone: 10 2024-11-06T04:40:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868018.53 2024-11-06T04:40:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:22Z- waiting for 1730868022.53, time is 1730868022.15 2024-11-06T04:40:22Z- AHRS_M2 msg: 1730868022.55 epoch sec; seqNo:699 2024-11-06T04:40:22Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.42 psi 2024-11-06T04:40:22Z- handleDepth()- Depth_Keller msg: 1730868022.56 epoch sec; seqNo:756; depth 47.98 2024-11-06T04:40:22Z- handleDVL() - DVL msg epochMillisec: 1730868022.59 2024-11-06T04:40:22Z- handleDvl() - RDI_Pathfinder msg: 1730868022.59 sec; seqNo:264; #beams:4 2024-11-06T04:40:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.40 , 43.32 , 42.88 , 51.50 2024-11-06T04:40:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , 0.12 2024-11-06T04:40:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868022.60 epoch sec; seqNo:756; 4061648.15 north; 594252.84 east; 36.70 lat; -121.94 lon; 47.98 depth; utmZone: 10 2024-11-06T04:40:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868018.53 2024-11-06T04:40:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:22Z- waiting for 1730868022.53, time is 1730868022.60 2024-11-06T04:40:22Z- LcmTrn::updateTrn() >>>> pose time 1730868022.60, meas time 1730868022.59 <<<< 2024-11-06T04:40:22Z- LcmTrn::updateTrn() equating pose 1730868022.59 and meas times 1730868022.59 2024-11-06T04:40:22Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868022.59 2024-11-06T04:40:22Z- TerrainNav::motionUpdate() - currEstimate = (4061648, 594252.84, 47.98) 2024-11-06T04:40:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:40:22Z- TerrainNav:: SubcloudNIS is 0.12, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:40:22Z- TNavPF::Projecting Measurements 2024-11-06T04:40:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:40:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:40:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:40:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:40:22Z- TNavFilter::4 good beams. 2024-11-06T04:40:22Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:40:22Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:40:22Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:40:22Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:40:22Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:40:22Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:40:22Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:40:22Z- TerrainMapOctree: (vehN, vehE ) = (4061648.15, 594252.84). 2024-11-06T04:40:22Z- TerrainMapOctree: Min Distance = 101.89. 2024-11-06T04:40:22Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:40:22Z- Forcing Subcloud Comparison 2024-11-06T04:40:22Z- Weighting particles with subcloud comparison 2024-11-06T04:40:22Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:22Z- meanExpectedMeasDiff: -0.031988 delta_rms_squared: 0.978315 alpha: 0.979966 2024-11-06T04:40:22Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:22Z- meanExpectedMeasDiff: 0.496205 delta_rms_squared: 0.740180 alpha: 0.973690 2024-11-06T04:40:22Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:22Z- meanExpectedMeasDiff: 0.736215 delta_rms_squared: 0.680270 alpha: 0.971440 2024-11-06T04:40:22Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:22Z- meanExpectedMeasDiff: -0.174695 delta_rms_squared: 1.082262 alpha: 0.981856 2024-11-06T04:40:22Z- Subcloud NIS: 0.112443 2024-11-06T04:40:22Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:40:22Z- TNavPF:: sumWeights = 0.724211 2024-11-06T04:40:22Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:40:22Z- TNavPF::First Term of Map Covariance Matrix is 0.44 2024-11-06T04:40:22Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T04:40:22Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868022.59 sec, ping # 0. 2024-11-06T04:40:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:40:22Z- variances([0], [2], and psi_berg) = 0.124810 0.120462 0.000000 2024-11-06T04:40:22Z- publishEstimatesDecoder() - at 1730868022.59 sec 2024-11-06T04:40:22Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:40:22Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061591.50 594256.58 54.86 2024-11-06T04:40:22Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061592.13 594256.93 54.97 lat/lon: 36.70 -121.94 2024-11-06T04:40:22Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.12 0.12 0.37 0.00 2024-11-06T04:40:22Z- AHRS_M2 msg: 1730868022.98 epoch sec; seqNo:700 2024-11-06T04:40:22Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.42 psi 2024-11-06T04:40:22Z- handleDepth()- Depth_Keller msg: 1730868022.98 epoch sec; seqNo:757; depth 48.05 2024-11-06T04:40:23Z- handleDVL() - DVL msg epochMillisec: 1730868022.99 2024-11-06T04:40:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:23Z- handleDvl() - RDI_Pathfinder msg: 1730868022.99 sec; seqNo:265; #beams:0 2024-11-06T04:40:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , 0.12 2024-11-06T04:40:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868023.02 epoch sec; seqNo:757; 4061648.15 north; 594252.84 east; 36.70 lat; -121.94 lon; 48.05 depth; utmZone: 10 2024-11-06T04:40:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868022.59 2024-11-06T04:40:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:23Z- waiting for 1730868026.59, time is 1730868023.02 2024-11-06T04:40:23Z- AHRS_M2 msg: 1730868023.35 epoch sec; seqNo:701 2024-11-06T04:40:23Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.42 psi 2024-11-06T04:40:23Z- handleDepth()- Depth_Keller msg: 1730868023.36 epoch sec; seqNo:758; depth 48.10 2024-11-06T04:40:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868023.38 epoch sec; seqNo:758; 4061648.53 north; 594252.84 east; 36.70 lat; -121.94 lon; 48.10 depth; utmZone: 10 2024-11-06T04:40:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868022.59 2024-11-06T04:40:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:23Z- waiting for 1730868026.59, time is 1730868023.38 2024-11-06T04:40:23Z- AHRS_M2 msg: 1730868023.76 epoch sec; seqNo:702 2024-11-06T04:40:23Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.42 psi 2024-11-06T04:40:23Z- handleDepth()- Depth_Keller msg: 1730868023.77 epoch sec; seqNo:759; depth 48.13 2024-11-06T04:40:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868023.77 epoch sec; seqNo:759; 4061648.53 north; 594252.84 east; 36.70 lat; -121.94 lon; 48.13 depth; utmZone: 10 2024-11-06T04:40:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868022.59 2024-11-06T04:40:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:23Z- waiting for 1730868026.59, time is 1730868023.77 2024-11-06T04:40:24Z- AHRS_M2 msg: 1730868024.16 epoch sec; seqNo:703 2024-11-06T04:40:24Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.42 psi 2024-11-06T04:40:24Z- handleDepth()- Depth_Keller msg: 1730868024.16 epoch sec; seqNo:760; depth 48.18 2024-11-06T04:40:24Z- handleDVL() - DVL msg epochMillisec: 1730868024.17 2024-11-06T04:40:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:24Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:24Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:24Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:24Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:24Z- handleDvl() - RDI_Pathfinder msg: 1730868024.17 sec; seqNo:266; #beams:0 2024-11-06T04:40:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , 0.12 2024-11-06T04:40:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868024.17 epoch sec; seqNo:760; 4061648.90 north; 594252.84 east; 36.70 lat; -121.94 lon; 48.18 depth; utmZone: 10 2024-11-06T04:40:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868022.59 2024-11-06T04:40:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:24Z- waiting for 1730868026.59, time is 1730868024.17 2024-11-06T04:40:24Z- AHRS_M2 msg: 1730868024.59 epoch sec; seqNo:704 2024-11-06T04:40:24Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.42 psi 2024-11-06T04:40:24Z- handleDepth()- Depth_Keller msg: 1730868024.59 epoch sec; seqNo:761; depth 48.24 2024-11-06T04:40:24Z- handleDVL() - DVL msg epochMillisec: 1730868024.61 2024-11-06T04:40:24Z- handleDvl() - RDI_Pathfinder msg: 1730868024.61 sec; seqNo:267; #beams:4 2024-11-06T04:40:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.40 , 42.88 , 42.88 , 51.28 2024-11-06T04:40:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , 0.08 2024-11-06T04:40:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868024.64 epoch sec; seqNo:761; 4061649.27 north; 594251.61 east; 36.70 lat; -121.94 lon; 48.24 depth; utmZone: 10 2024-11-06T04:40:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868022.59 2024-11-06T04:40:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:24Z- waiting for 1730868026.59, time is 1730868024.64 2024-11-06T04:40:24Z- AHRS_M2 msg: 1730868024.97 epoch sec; seqNo:705 2024-11-06T04:40:24Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 5.41 psi 2024-11-06T04:40:24Z- handleDepth()- Depth_Keller msg: 1730868024.97 epoch sec; seqNo:762; depth 48.28 2024-11-06T04:40:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868025.00 epoch sec; seqNo:762; 4061649.27 north; 594251.61 east; 36.70 lat; -121.94 lon; 48.28 depth; utmZone: 10 2024-11-06T04:40:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868022.59 2024-11-06T04:40:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:25Z- waiting for 1730868026.59, time is 1730868025.00 2024-11-06T04:40:25Z- AHRS_M2 msg: 1730868025.37 epoch sec; seqNo:706 2024-11-06T04:40:25Z- AHRS_M2 msg: 0.11 phi; -0.19 theta; 5.41 psi 2024-11-06T04:40:25Z- handleDepth()- Depth_Keller msg: 1730868025.38 epoch sec; seqNo:763; depth 48.33 2024-11-06T04:40:25Z- handleDVL() - DVL msg epochMillisec: 1730868025.38 2024-11-06T04:40:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:25Z- handleDvl() - RDI_Pathfinder msg: 1730868025.38 sec; seqNo:268; #beams:0 2024-11-06T04:40:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.03 , 0.08 2024-11-06T04:40:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868025.39 epoch sec; seqNo:763; 4061649.65 north; 594251.61 east; 36.70 lat; -121.94 lon; 48.33 depth; utmZone: 10 2024-11-06T04:40:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868022.59 2024-11-06T04:40:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:25Z- waiting for 1730868026.59, time is 1730868025.39 2024-11-06T04:40:25Z- AHRS_M2 msg: 1730868025.78 epoch sec; seqNo:707 2024-11-06T04:40:25Z- AHRS_M2 msg: 0.11 phi; -0.19 theta; 5.41 psi 2024-11-06T04:40:25Z- handleDepth()- Depth_Keller msg: 1730868025.79 epoch sec; seqNo:764; depth 48.36 2024-11-06T04:40:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868025.80 epoch sec; seqNo:764; 4061649.65 north; 594251.61 east; 36.70 lat; -121.94 lon; 48.36 depth; utmZone: 10 2024-11-06T04:40:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868022.59 2024-11-06T04:40:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:25Z- waiting for 1730868026.59, time is 1730868025.80 2024-11-06T04:40:26Z- AHRS_M2 msg: 1730868026.18 epoch sec; seqNo:708 2024-11-06T04:40:26Z- AHRS_M2 msg: 0.11 phi; -0.19 theta; 5.41 psi 2024-11-06T04:40:26Z- handleDepth()- Depth_Keller msg: 1730868026.19 epoch sec; seqNo:765; depth 48.40 2024-11-06T04:40:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868026.20 epoch sec; seqNo:765; 4061650.03 north; 594251.60 east; 36.70 lat; -121.94 lon; 48.40 depth; utmZone: 10 2024-11-06T04:40:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868022.59 2024-11-06T04:40:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:26Z- waiting for 1730868026.59, time is 1730868026.20 2024-11-06T04:40:26Z- AHRS_M2 msg: 1730868026.59 epoch sec; seqNo:709 2024-11-06T04:40:26Z- AHRS_M2 msg: 0.11 phi; -0.19 theta; 5.41 psi 2024-11-06T04:40:26Z- handleDepth()- Depth_Keller msg: 1730868026.59 epoch sec; seqNo:766; depth 48.43 2024-11-06T04:40:26Z- handleDVL() - DVL msg epochMillisec: 1730868026.63 2024-11-06T04:40:26Z- handleDvl() - RDI_Pathfinder msg: 1730868026.63 sec; seqNo:269; #beams:4 2024-11-06T04:40:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.73 , 42.88 , 42.66 , 51.28 2024-11-06T04:40:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , 0.01 2024-11-06T04:40:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868026.64 epoch sec; seqNo:766; 4061650.39 north; 594250.38 east; 36.70 lat; -121.94 lon; 48.43 depth; utmZone: 10 2024-11-06T04:40:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868022.59 2024-11-06T04:40:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:26Z- waiting for 1730868026.59, time is 1730868026.64 2024-11-06T04:40:26Z- LcmTrn::updateTrn() >>>> pose time 1730868026.64, meas time 1730868026.63 <<<< 2024-11-06T04:40:26Z- LcmTrn::updateTrn() equating pose 1730868026.63 and meas times 1730868026.63 2024-11-06T04:40:26Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868026.63 2024-11-06T04:40:26Z- TerrainNav::motionUpdate() - currEstimate = (4061650, 594250.38, 48.43) 2024-11-06T04:40:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:40:26Z- TerrainNav:: SubcloudNIS is 0.11, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:40:26Z- TNavPF::Projecting Measurements 2024-11-06T04:40:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:40:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:40:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:40:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:40:26Z- TNavFilter::4 good beams. 2024-11-06T04:40:26Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:40:26Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:40:26Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:40:26Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:40:26Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:40:26Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:40:26Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:40:26Z- TerrainMapOctree: (vehN, vehE ) = (4061650.39, 594250.38). 2024-11-06T04:40:26Z- TerrainMapOctree: Min Distance = 99.61. 2024-11-06T04:40:26Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:40:26Z- Forcing Subcloud Comparison 2024-11-06T04:40:26Z- Weighting particles with subcloud comparison 2024-11-06T04:40:26Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:26Z- meanExpectedMeasDiff: 0.247980 delta_rms_squared: 0.986607 alpha: 0.980131 2024-11-06T04:40:26Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:26Z- meanExpectedMeasDiff: 0.287636 delta_rms_squared: 0.767434 alpha: 0.974601 2024-11-06T04:40:26Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:26Z- meanExpectedMeasDiff: 0.843570 delta_rms_squared: 0.701652 alpha: 0.972286 2024-11-06T04:40:26Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:26Z- meanExpectedMeasDiff: 0.455293 delta_rms_squared: 1.097005 alpha: 0.982095 2024-11-06T04:40:26Z- Subcloud NIS: 0.113044 2024-11-06T04:40:26Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:40:26Z- TNavPF:: sumWeights = 0.720374 2024-11-06T04:40:26Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:40:26Z- TNavPF::First Term of Map Covariance Matrix is 0.43 2024-11-06T04:40:26Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T04:40:26Z- TerrainNav::Resampling particle filter... 2024-11-06T04:40:26Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868026.63 sec, ping # 0. 2024-11-06T04:40:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:40:26Z- variances([0], [2], and psi_berg) = 0.125599 0.121424 0.000000 2024-11-06T04:40:26Z- publishEstimatesDecoder() - at 1730868026.63 sec 2024-11-06T04:40:26Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:40:26Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061594.18 594254.41 55.83 2024-11-06T04:40:26Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061594.37 594254.47 55.26 lat/lon: 36.70 -121.94 2024-11-06T04:40:26Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.13 0.12 0.37 0.00 2024-11-06T04:40:27Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:40:27Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:40:27Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:40:27Z- AHRS_M2 msg: 1730868026.98 epoch sec; seqNo:710 2024-11-06T04:40:27Z- AHRS_M2 msg: 0.11 phi; -0.19 theta; 5.41 psi 2024-11-06T04:40:27Z- handleDepth()- Depth_Keller msg: 1730868027.01 epoch sec; seqNo:767; depth 48.47 2024-11-06T04:40:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868027.03 epoch sec; seqNo:767; 4061650.39 north; 594250.38 east; 36.70 lat; -121.94 lon; 48.47 depth; utmZone: 10 2024-11-06T04:40:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868026.63 2024-11-06T04:40:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:27Z- waiting for 1730868030.63, time is 1730868027.03 2024-11-06T04:40:27Z- AHRS_M2 msg: 1730868027.39 epoch sec; seqNo:711 2024-11-06T04:40:27Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.40 psi 2024-11-06T04:40:27Z- handleDepth()- Depth_Keller msg: 1730868027.40 epoch sec; seqNo:768; depth 48.50 2024-11-06T04:40:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868027.41 epoch sec; seqNo:768; 4061650.77 north; 594250.37 east; 36.70 lat; -121.94 lon; 48.50 depth; utmZone: 10 2024-11-06T04:40:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868026.63 2024-11-06T04:40:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:27Z- waiting for 1730868030.63, time is 1730868027.41 2024-11-06T04:40:27Z- AHRS_M2 msg: 1730868027.80 epoch sec; seqNo:712 2024-11-06T04:40:27Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.40 psi 2024-11-06T04:40:27Z- handleDepth()- Depth_Keller msg: 1730868027.80 epoch sec; seqNo:769; depth 48.52 2024-11-06T04:40:27Z- handleDVL() - DVL msg epochMillisec: 1730868027.81 2024-11-06T04:40:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:27Z- handleDvl() - RDI_Pathfinder msg: 1730868027.81 sec; seqNo:270; #beams:0 2024-11-06T04:40:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , 0.01 2024-11-06T04:40:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868027.83 epoch sec; seqNo:769; 4061650.77 north; 594250.37 east; 36.70 lat; -121.94 lon; 48.52 depth; utmZone: 10 2024-11-06T04:40:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868026.63 2024-11-06T04:40:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:27Z- waiting for 1730868030.63, time is 1730868027.83 2024-11-06T04:40:28Z- AHRS_M2 msg: 1730868028.20 epoch sec; seqNo:713 2024-11-06T04:40:28Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.40 psi 2024-11-06T04:40:28Z- handleDepth()- Depth_Keller msg: 1730868028.21 epoch sec; seqNo:770; depth 48.58 2024-11-06T04:40:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868028.21 epoch sec; seqNo:770; 4061651.14 north; 594249.15 east; 36.70 lat; -121.94 lon; 48.58 depth; utmZone: 10 2024-11-06T04:40:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868026.63 2024-11-06T04:40:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:28Z- waiting for 1730868030.63, time is 1730868028.21 2024-11-06T04:40:28Z- AHRS_M2 msg: 1730868028.61 epoch sec; seqNo:714 2024-11-06T04:40:28Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.40 psi 2024-11-06T04:40:28Z- handleDepth()- Depth_Keller msg: 1730868028.62 epoch sec; seqNo:771; depth 48.62 2024-11-06T04:40:28Z- handleDVL() - DVL msg epochMillisec: 1730868028.65 2024-11-06T04:40:28Z- handleDvl() - RDI_Pathfinder msg: 1730868028.65 sec; seqNo:271; #beams:4 2024-11-06T04:40:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.29 , 42.66 , 42.22 , 50.40 2024-11-06T04:40:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.01 , -0.01 2024-11-06T04:40:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868028.68 epoch sec; seqNo:771; 4061651.52 north; 594249.15 east; 36.70 lat; -121.94 lon; 48.62 depth; utmZone: 10 2024-11-06T04:40:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868026.63 2024-11-06T04:40:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:28Z- waiting for 1730868030.63, time is 1730868028.68 2024-11-06T04:40:29Z- AHRS_M2 msg: 1730868029.01 epoch sec; seqNo:715 2024-11-06T04:40:29Z- AHRS_M2 msg: 0.11 phi; -0.18 theta; 5.40 psi 2024-11-06T04:40:29Z- handleDepth()- Depth_Keller msg: 1730868029.01 epoch sec; seqNo:772; depth 48.65 2024-11-06T04:40:29Z- handleDVL() - DVL msg epochMillisec: 1730868029.01 2024-11-06T04:40:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:29Z- handleDvl() - RDI_Pathfinder msg: 1730868029.01 sec; seqNo:272; #beams:0 2024-11-06T04:40:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.01 , -0.01 2024-11-06T04:40:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868029.03 epoch sec; seqNo:772; 4061651.52 north; 594249.15 east; 36.70 lat; -121.94 lon; 48.65 depth; utmZone: 10 2024-11-06T04:40:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868026.63 2024-11-06T04:40:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:29Z- waiting for 1730868030.63, time is 1730868029.03 2024-11-06T04:40:29Z- AHRS_M2 msg: 1730868029.41 epoch sec; seqNo:716 2024-11-06T04:40:29Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.39 psi 2024-11-06T04:40:29Z- handleDepth()- Depth_Keller msg: 1730868029.42 epoch sec; seqNo:773; depth 48.69 2024-11-06T04:40:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868029.43 epoch sec; seqNo:773; 4061651.90 north; 594249.14 east; 36.70 lat; -121.94 lon; 48.69 depth; utmZone: 10 2024-11-06T04:40:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868026.63 2024-11-06T04:40:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:29Z- waiting for 1730868030.63, time is 1730868029.43 2024-11-06T04:40:29Z- AHRS_M2 msg: 1730868029.82 epoch sec; seqNo:717 2024-11-06T04:40:29Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.39 psi 2024-11-06T04:40:29Z- handleDepth()- Depth_Keller msg: 1730868029.83 epoch sec; seqNo:774; depth 48.73 2024-11-06T04:40:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868029.85 epoch sec; seqNo:774; 4061651.88 north; 594247.92 east; 36.70 lat; -121.94 lon; 48.73 depth; utmZone: 10 2024-11-06T04:40:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868026.63 2024-11-06T04:40:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:29Z- waiting for 1730868030.63, time is 1730868029.85 2024-11-06T04:40:30Z- AHRS_M2 msg: 1730868030.22 epoch sec; seqNo:718 2024-11-06T04:40:30Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.39 psi 2024-11-06T04:40:30Z- handleDepth()- Depth_Keller msg: 1730868030.23 epoch sec; seqNo:775; depth 48.77 2024-11-06T04:40:30Z- handleDVL() - DVL msg epochMillisec: 1730868030.23 2024-11-06T04:40:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:30Z- handleDvl() - RDI_Pathfinder msg: 1730868030.23 sec; seqNo:273; #beams:0 2024-11-06T04:40:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.01 , -0.01 2024-11-06T04:40:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868030.24 epoch sec; seqNo:775; 4061652.26 north; 594247.92 east; 36.70 lat; -121.94 lon; 48.77 depth; utmZone: 10 2024-11-06T04:40:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868026.63 2024-11-06T04:40:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:30Z- waiting for 1730868030.63, time is 1730868030.24 2024-11-06T04:40:30Z- AHRS_M2 msg: 1730868030.63 epoch sec; seqNo:719 2024-11-06T04:40:30Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.38 psi 2024-11-06T04:40:30Z- handleDepth()- Depth_Keller msg: 1730868030.63 epoch sec; seqNo:776; depth 48.79 2024-11-06T04:40:30Z- handleDVL() - DVL msg epochMillisec: 1730868030.65 2024-11-06T04:40:30Z- handleDvl() - RDI_Pathfinder msg: 1730868030.65 sec; seqNo:274; #beams:4 2024-11-06T04:40:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.84 , 42.21 , 42.21 , 49.95 2024-11-06T04:40:30Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , -0.04 , 0.03 2024-11-06T04:40:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868030.69 epoch sec; seqNo:776; 4061652.64 north; 594247.91 east; 36.70 lat; -121.94 lon; 48.79 depth; utmZone: 10 2024-11-06T04:40:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868026.63 2024-11-06T04:40:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:30Z- waiting for 1730868030.63, time is 1730868030.69 2024-11-06T04:40:30Z- LcmTrn::updateTrn() >>>> pose time 1730868030.69, meas time 1730868030.65 <<<< 2024-11-06T04:40:30Z- LcmTrn::updateTrn() equating pose 1730868030.65 and meas times 1730868030.65 2024-11-06T04:40:30Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868030.65 2024-11-06T04:40:30Z- TerrainNav::motionUpdate() - currEstimate = (4061653, 594247.91, 48.79) 2024-11-06T04:40:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:40:30Z- TerrainNav:: SubcloudNIS is 0.11, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:40:30Z- TNavPF::Projecting Measurements 2024-11-06T04:40:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:40:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:40:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:40:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:40:30Z- TNavFilter::4 good beams. 2024-11-06T04:40:30Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:40:30Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:40:30Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:40:30Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:40:30Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:40:30Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:40:30Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:40:30Z- TerrainMapOctree: (vehN, vehE ) = (4061652.64, 594247.91). 2024-11-06T04:40:30Z- TerrainMapOctree: Min Distance = 97.38. 2024-11-06T04:40:30Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:40:30Z- Forcing Subcloud Comparison 2024-11-06T04:40:30Z- Weighting particles with subcloud comparison 2024-11-06T04:40:30Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:30Z- meanExpectedMeasDiff: -0.361357 delta_rms_squared: 0.982012 alpha: 0.980040 2024-11-06T04:40:30Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:30Z- meanExpectedMeasDiff: 0.039837 delta_rms_squared: 0.757921 alpha: 0.974290 2024-11-06T04:40:30Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:30Z- meanExpectedMeasDiff: 0.318533 delta_rms_squared: 0.709513 alpha: 0.972584 2024-11-06T04:40:30Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:30Z- meanExpectedMeasDiff: 0.128381 delta_rms_squared: 1.061631 alpha: 0.981509 2024-11-06T04:40:30Z- Subcloud NIS: 0.111108 2024-11-06T04:40:30Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:40:30Z- TNavPF:: sumWeights = 0.753348 2024-11-06T04:40:30Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:40:30Z- TNavPF::First Term of Map Covariance Matrix is 0.46 2024-11-06T04:40:30Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T04:40:30Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868030.65 sec, ping # 0. 2024-11-06T04:40:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:40:30Z- variances([0], [2], and psi_berg) = 0.127657 0.123282 0.000000 2024-11-06T04:40:30Z- publishEstimatesDecoder() - at 1730868030.65 sec 2024-11-06T04:40:30Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:40:30Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061596.41 594251.70 55.58 2024-11-06T04:40:30Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061596.62 594252.00 55.59 lat/lon: 36.70 -121.94 2024-11-06T04:40:30Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.13 0.12 0.38 0.00 2024-11-06T04:40:31Z- AHRS_M2 msg: 1730868031.03 epoch sec; seqNo:720 2024-11-06T04:40:31Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.38 psi 2024-11-06T04:40:31Z- handleDepth()- Depth_Keller msg: 1730868031.05 epoch sec; seqNo:777; depth 48.84 2024-11-06T04:40:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868031.07 epoch sec; seqNo:777; 4061652.64 north; 594247.91 east; 36.70 lat; -121.94 lon; 48.84 depth; utmZone: 10 2024-11-06T04:40:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868030.65 2024-11-06T04:40:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:31Z- waiting for 1730868034.65, time is 1730868031.07 2024-11-06T04:40:31Z- AHRS_M2 msg: 1730868031.43 epoch sec; seqNo:721 2024-11-06T04:40:31Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.38 psi 2024-11-06T04:40:31Z- handleDepth()- Depth_Keller msg: 1730868031.43 epoch sec; seqNo:778; depth 48.88 2024-11-06T04:40:31Z- handleDVL() - DVL msg epochMillisec: 1730868031.44 2024-11-06T04:40:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:31Z- handleDvl() - RDI_Pathfinder msg: 1730868031.44 sec; seqNo:275; #beams:0 2024-11-06T04:40:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:31Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , -0.04 , 0.03 2024-11-06T04:40:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868031.45 epoch sec; seqNo:778; 4061653.00 north; 594246.69 east; 36.70 lat; -121.94 lon; 48.88 depth; utmZone: 10 2024-11-06T04:40:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868030.65 2024-11-06T04:40:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:31Z- waiting for 1730868034.65, time is 1730868031.45 2024-11-06T04:40:31Z- AHRS_M2 msg: 1730868031.84 epoch sec; seqNo:722 2024-11-06T04:40:31Z- AHRS_M2 msg: 0.11 phi; -0.17 theta; 5.38 psi 2024-11-06T04:40:31Z- handleDepth()- Depth_Keller msg: 1730868031.85 epoch sec; seqNo:779; depth 48.93 2024-11-06T04:40:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868031.86 epoch sec; seqNo:779; 4061653.38 north; 594246.68 east; 36.70 lat; -121.94 lon; 48.93 depth; utmZone: 10 2024-11-06T04:40:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868030.65 2024-11-06T04:40:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:32Z- waiting for 1730868034.65, time is 1730868031.86 2024-11-06T04:40:32Z- AHRS_M2 msg: 1730868032.24 epoch sec; seqNo:723 2024-11-06T04:40:32Z- AHRS_M2 msg: 0.11 phi; -0.16 theta; 5.37 psi 2024-11-06T04:40:32Z- handleDepth()- Depth_Keller msg: 1730868032.24 epoch sec; seqNo:780; depth 48.98 2024-11-06T04:40:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868032.26 epoch sec; seqNo:780; 4061653.38 north; 594246.68 east; 36.70 lat; -121.94 lon; 48.98 depth; utmZone: 10 2024-11-06T04:40:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868030.65 2024-11-06T04:40:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:32Z- waiting for 1730868034.65, time is 1730868032.26 2024-11-06T04:40:32Z- AHRS_M2 msg: 1730868032.64 epoch sec; seqNo:724 2024-11-06T04:40:32Z- AHRS_M2 msg: 0.11 phi; -0.16 theta; 5.37 psi 2024-11-06T04:40:32Z- handleDepth()- Depth_Keller msg: 1730868032.65 epoch sec; seqNo:781; depth 49.01 2024-11-06T04:40:32Z- handleDVL() - DVL msg epochMillisec: 1730868032.67 2024-11-06T04:40:32Z- handleDvl() - RDI_Pathfinder msg: 1730868032.67 sec; seqNo:276; #beams:4 2024-11-06T04:40:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.84 , 41.99 , 42.43 , 48.84 2024-11-06T04:40:32Z- handleDvl() - RDI_Pathfinder msg: velocities 1.04 , -0.03 , 0.09 2024-11-06T04:40:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868032.70 epoch sec; seqNo:781; 4061653.76 north; 594246.68 east; 36.70 lat; -121.94 lon; 49.01 depth; utmZone: 10 2024-11-06T04:40:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868030.65 2024-11-06T04:40:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:32Z- waiting for 1730868034.65, time is 1730868032.70 2024-11-06T04:40:33Z- AHRS_M2 msg: 1730868033.05 epoch sec; seqNo:725 2024-11-06T04:40:33Z- AHRS_M2 msg: 0.11 phi; -0.16 theta; 5.37 psi 2024-11-06T04:40:33Z- handleDepth()- Depth_Keller msg: 1730868033.05 epoch sec; seqNo:782; depth 49.06 2024-11-06T04:40:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868033.07 epoch sec; seqNo:782; 4061654.13 north; 594245.46 east; 36.70 lat; -121.94 lon; 49.06 depth; utmZone: 10 2024-11-06T04:40:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868030.65 2024-11-06T04:40:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:33Z- waiting for 1730868034.65, time is 1730868033.07 2024-11-06T04:40:33Z- AHRS_M2 msg: 1730868033.45 epoch sec; seqNo:726 2024-11-06T04:40:33Z- AHRS_M2 msg: 0.11 phi; -0.16 theta; 5.37 psi 2024-11-06T04:40:33Z- handleDepth()- Depth_Keller msg: 1730868033.46 epoch sec; seqNo:783; depth 49.11 2024-11-06T04:40:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868033.47 epoch sec; seqNo:783; 4061654.13 north; 594245.46 east; 36.70 lat; -121.94 lon; 49.11 depth; utmZone: 10 2024-11-06T04:40:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868030.65 2024-11-06T04:40:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:33Z- waiting for 1730868034.65, time is 1730868033.47 2024-11-06T04:40:33Z- AHRS_M2 msg: 1730868033.86 epoch sec; seqNo:727 2024-11-06T04:40:33Z- AHRS_M2 msg: 0.11 phi; -0.16 theta; 5.37 psi 2024-11-06T04:40:33Z- handleDepth()- Depth_Keller msg: 1730868033.87 epoch sec; seqNo:784; depth 49.14 2024-11-06T04:40:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868033.88 epoch sec; seqNo:784; 4061654.51 north; 594245.45 east; 36.70 lat; -121.94 lon; 49.14 depth; utmZone: 10 2024-11-06T04:40:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868030.65 2024-11-06T04:40:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:34Z- waiting for 1730868034.65, time is 1730868033.88 2024-11-06T04:40:34Z- AHRS_M2 msg: 1730868034.26 epoch sec; seqNo:728 2024-11-06T04:40:34Z- AHRS_M2 msg: 0.12 phi; -0.16 theta; 5.36 psi 2024-11-06T04:40:34Z- handleDepth()- Depth_Keller msg: 1730868034.26 epoch sec; seqNo:785; depth 49.19 2024-11-06T04:40:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868034.27 epoch sec; seqNo:785; 4061654.49 north; 594244.23 east; 36.70 lat; -121.94 lon; 49.19 depth; utmZone: 10 2024-11-06T04:40:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868030.65 2024-11-06T04:40:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:34Z- waiting for 1730868034.65, time is 1730868034.27 2024-11-06T04:40:34Z- AHRS_M2 msg: 1730868034.67 epoch sec; seqNo:729 2024-11-06T04:40:34Z- AHRS_M2 msg: 0.11 phi; -0.16 theta; 5.36 psi 2024-11-06T04:40:34Z- handleDepth()- Depth_Keller msg: 1730868034.69 epoch sec; seqNo:786; depth 49.24 2024-11-06T04:40:34Z- handleDVL() - DVL msg epochMillisec: 1730868034.71 2024-11-06T04:40:34Z- handleDvl() - RDI_Pathfinder msg: 1730868034.71 sec; seqNo:277; #beams:4 2024-11-06T04:40:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.84 , 41.99 , 42.43 , 48.84 2024-11-06T04:40:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.01 , 0.12 2024-11-06T04:40:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868034.72 epoch sec; seqNo:786; 4061654.87 north; 594244.23 east; 36.70 lat; -121.94 lon; 49.24 depth; utmZone: 10 2024-11-06T04:40:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868030.65 2024-11-06T04:40:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:34Z- waiting for 1730868034.65, time is 1730868034.72 2024-11-06T04:40:34Z- LcmTrn::updateTrn() >>>> pose time 1730868034.72, meas time 1730868034.71 <<<< 2024-11-06T04:40:34Z- LcmTrn::updateTrn() equating pose 1730868034.71 and meas times 1730868034.71 2024-11-06T04:40:34Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868034.71 2024-11-06T04:40:34Z- TerrainNav::motionUpdate() - currEstimate = (4061655, 594244.23, 49.24) 2024-11-06T04:40:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:40:34Z- TerrainNav:: SubcloudNIS is 0.11, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:40:34Z- TNavPF::Projecting Measurements 2024-11-06T04:40:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:40:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:40:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:40:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:40:34Z- TNavFilter::4 good beams. 2024-11-06T04:40:34Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:40:34Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:40:34Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:40:34Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:40:34Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:40:34Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:40:34Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:40:34Z- TerrainMapOctree: (vehN, vehE ) = (4061654.87, 594244.23). 2024-11-06T04:40:34Z- TerrainMapOctree: Min Distance = 95.30. 2024-11-06T04:40:34Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:40:34Z- Forcing Subcloud Comparison 2024-11-06T04:40:34Z- Weighting particles with subcloud comparison 2024-11-06T04:40:34Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:34Z- meanExpectedMeasDiff: 1.436254 delta_rms_squared: 0.982864 alpha: 0.980057 2024-11-06T04:40:34Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:34Z- meanExpectedMeasDiff: 0.451469 delta_rms_squared: 0.752349 alpha: 0.974105 2024-11-06T04:40:34Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:34Z- meanExpectedMeasDiff: 0.788433 delta_rms_squared: 0.717059 alpha: 0.972865 2024-11-06T04:40:34Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:34Z- meanExpectedMeasDiff: 0.102337 delta_rms_squared: 1.039524 alpha: 0.981124 2024-11-06T04:40:34Z- Subcloud NIS: 0.115628 2024-11-06T04:40:34Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:40:34Z- TNavPF:: sumWeights = 0.635279 2024-11-06T04:40:34Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:40:34Z- TNavPF::First Term of Map Covariance Matrix is 0.48 2024-11-06T04:40:34Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T04:40:34Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868034.71 sec, ping # 0. 2024-11-06T04:40:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:40:34Z- variances([0], [2], and psi_berg) = 0.130875 0.125374 0.000000 2024-11-06T04:40:34Z- publishEstimatesDecoder() - at 1730868034.71 sec 2024-11-06T04:40:34Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:40:34Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061598.28 594249.03 55.45 2024-11-06T04:40:34Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061598.85 594248.31 55.81 lat/lon: 36.70 -121.94 2024-11-06T04:40:34Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.13 0.13 0.37 0.00 2024-11-06T04:40:35Z- AHRS_M2 msg: 1730868035.09 epoch sec; seqNo:730 2024-11-06T04:40:35Z- AHRS_M2 msg: 0.11 phi; -0.16 theta; 5.36 psi 2024-11-06T04:40:35Z- handleDepth()- Depth_Keller msg: 1730868035.11 epoch sec; seqNo:787; depth 49.29 2024-11-06T04:40:35Z- handleDVL() - DVL msg epochMillisec: 1730868035.11 2024-11-06T04:40:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:35Z- handleDvl() - RDI_Pathfinder msg: 1730868035.11 sec; seqNo:278; #beams:0 2024-11-06T04:40:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.01 , 0.12 2024-11-06T04:40:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868035.13 epoch sec; seqNo:787; 4061655.25 north; 594244.22 east; 36.70 lat; -121.94 lon; 49.29 depth; utmZone: 10 2024-11-06T04:40:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868034.71 2024-11-06T04:40:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:35Z- waiting for 1730868038.71, time is 1730868035.13 2024-11-06T04:40:35Z- AHRS_M2 msg: 1730868035.47 epoch sec; seqNo:731 2024-11-06T04:40:35Z- AHRS_M2 msg: 0.11 phi; -0.15 theta; 5.36 psi 2024-11-06T04:40:35Z- handleDepth()- Depth_Keller msg: 1730868035.48 epoch sec; seqNo:788; depth 49.33 2024-11-06T04:40:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868035.49 epoch sec; seqNo:788; 4061655.25 north; 594244.22 east; 36.70 lat; -121.94 lon; 49.33 depth; utmZone: 10 2024-11-06T04:40:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868034.71 2024-11-06T04:40:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:35Z- waiting for 1730868038.71, time is 1730868035.49 2024-11-06T04:40:35Z- AHRS_M2 msg: 1730868035.88 epoch sec; seqNo:732 2024-11-06T04:40:35Z- AHRS_M2 msg: 0.11 phi; -0.15 theta; 5.36 psi 2024-11-06T04:40:35Z- handleDepth()- Depth_Keller msg: 1730868035.88 epoch sec; seqNo:789; depth 49.36 2024-11-06T04:40:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868035.89 epoch sec; seqNo:789; 4061655.62 north; 594243.00 east; 36.70 lat; -121.95 lon; 49.36 depth; utmZone: 10 2024-11-06T04:40:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868034.71 2024-11-06T04:40:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:36Z- waiting for 1730868038.71, time is 1730868035.89 2024-11-06T04:40:36Z- AHRS_M2 msg: 1730868036.28 epoch sec; seqNo:733 2024-11-06T04:40:36Z- AHRS_M2 msg: 0.11 phi; -0.15 theta; 5.36 psi 2024-11-06T04:40:36Z- handleDepth()- Depth_Keller msg: 1730868036.29 epoch sec; seqNo:790; depth 49.41 2024-11-06T04:40:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868036.31 epoch sec; seqNo:790; 4061655.62 north; 594243.00 east; 36.70 lat; -121.95 lon; 49.41 depth; utmZone: 10 2024-11-06T04:40:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868034.71 2024-11-06T04:40:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:36Z- waiting for 1730868038.71, time is 1730868036.31 2024-11-06T04:40:36Z- AHRS_M2 msg: 1730868036.68 epoch sec; seqNo:734 2024-11-06T04:40:36Z- AHRS_M2 msg: 0.11 phi; -0.15 theta; 5.36 psi 2024-11-06T04:40:36Z- handleDepth()- Depth_Keller msg: 1730868036.69 epoch sec; seqNo:791; depth 49.45 2024-11-06T04:40:36Z- handleDVL() - DVL msg epochMillisec: 1730868036.72 2024-11-06T04:40:36Z- handleDvl() - RDI_Pathfinder msg: 1730868036.72 sec; seqNo:279; #beams:4 2024-11-06T04:40:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.62 , 41.99 , 42.43 , 48.62 2024-11-06T04:40:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , 0.11 2024-11-06T04:40:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868036.73 epoch sec; seqNo:791; 4061656.00 north; 594242.99 east; 36.70 lat; -121.95 lon; 49.45 depth; utmZone: 10 2024-11-06T04:40:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868034.71 2024-11-06T04:40:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:36Z- waiting for 1730868038.71, time is 1730868036.73 2024-11-06T04:40:37Z- AHRS_M2 msg: 1730868037.09 epoch sec; seqNo:735 2024-11-06T04:40:37Z- AHRS_M2 msg: 0.11 phi; -0.15 theta; 5.36 psi 2024-11-06T04:40:37Z- handleDepth()- Depth_Keller msg: 1730868037.10 epoch sec; seqNo:792; depth 49.48 2024-11-06T04:40:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868037.11 epoch sec; seqNo:792; 4061656.37 north; 594242.99 east; 36.70 lat; -121.95 lon; 49.48 depth; utmZone: 10 2024-11-06T04:40:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868034.71 2024-11-06T04:40:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:37Z- waiting for 1730868038.71, time is 1730868037.11 2024-11-06T04:40:37Z- AHRS_M2 msg: 1730868037.49 epoch sec; seqNo:736 2024-11-06T04:40:37Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.36 psi 2024-11-06T04:40:37Z- handleDepth()- Depth_Keller msg: 1730868037.50 epoch sec; seqNo:793; depth 49.54 2024-11-06T04:40:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868037.52 epoch sec; seqNo:793; 4061656.36 north; 594241.77 east; 36.70 lat; -121.95 lon; 49.54 depth; utmZone: 10 2024-11-06T04:40:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868034.71 2024-11-06T04:40:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:37Z- waiting for 1730868038.71, time is 1730868037.52 2024-11-06T04:40:37Z- AHRS_M2 msg: 1730868037.90 epoch sec; seqNo:737 2024-11-06T04:40:37Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.36 psi 2024-11-06T04:40:37Z- handleDepth()- Depth_Keller msg: 1730868037.90 epoch sec; seqNo:794; depth 49.56 2024-11-06T04:40:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868037.91 epoch sec; seqNo:794; 4061656.74 north; 594241.77 east; 36.70 lat; -121.95 lon; 49.56 depth; utmZone: 10 2024-11-06T04:40:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868034.71 2024-11-06T04:40:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:38Z- waiting for 1730868038.71, time is 1730868037.91 2024-11-06T04:40:38Z- AHRS_M2 msg: 1730868038.30 epoch sec; seqNo:738 2024-11-06T04:40:38Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.36 psi 2024-11-06T04:40:38Z- handleDepth()- Depth_Keller msg: 1730868038.32 epoch sec; seqNo:795; depth 49.59 2024-11-06T04:40:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868038.33 epoch sec; seqNo:795; 4061656.74 north; 594241.77 east; 36.70 lat; -121.95 lon; 49.59 depth; utmZone: 10 2024-11-06T04:40:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868034.71 2024-11-06T04:40:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:38Z- waiting for 1730868038.71, time is 1730868038.33 2024-11-06T04:40:38Z- AHRS_M2 msg: 1730868038.71 epoch sec; seqNo:739 2024-11-06T04:40:38Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.37 psi 2024-11-06T04:40:38Z- handleDepth()- Depth_Keller msg: 1730868038.71 epoch sec; seqNo:796; depth 49.62 2024-11-06T04:40:38Z- handleDVL() - DVL msg epochMillisec: 1730868038.75 2024-11-06T04:40:38Z- handleDvl() - RDI_Pathfinder msg: 1730868038.75 sec; seqNo:280; #beams:4 2024-11-06T04:40:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.40 , 41.99 , 42.43 , 48.62 2024-11-06T04:40:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , 0.06 2024-11-06T04:40:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868038.77 epoch sec; seqNo:796; 4061657.12 north; 594241.76 east; 36.70 lat; -121.95 lon; 49.62 depth; utmZone: 10 2024-11-06T04:40:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868034.71 2024-11-06T04:40:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:38Z- waiting for 1730868038.71, time is 1730868038.77 2024-11-06T04:40:38Z- LcmTrn::updateTrn() >>>> pose time 1730868038.77, meas time 1730868038.75 <<<< 2024-11-06T04:40:38Z- LcmTrn::updateTrn() equating pose 1730868038.75 and meas times 1730868038.75 2024-11-06T04:40:38Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868038.75 2024-11-06T04:40:38Z- TerrainNav::motionUpdate() - currEstimate = (4061657, 594241.76, 49.62) 2024-11-06T04:40:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:40:38Z- TerrainNav:: SubcloudNIS is 0.12, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:40:38Z- TNavPF::Projecting Measurements 2024-11-06T04:40:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:40:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:40:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:40:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:40:38Z- TNavFilter::4 good beams. 2024-11-06T04:40:38Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:40:38Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:40:38Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:40:38Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:40:38Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:40:38Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:40:38Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:40:38Z- TerrainMapOctree: (vehN, vehE ) = (4061657.12, 594241.76). 2024-11-06T04:40:38Z- TerrainMapOctree: Min Distance = 93.25. 2024-11-06T04:40:38Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:40:38Z- Forcing Subcloud Comparison 2024-11-06T04:40:38Z- Weighting particles with subcloud comparison 2024-11-06T04:40:38Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:38Z- meanExpectedMeasDiff: 1.106705 delta_rms_squared: 0.955354 alpha: 0.979495 2024-11-06T04:40:39Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:39Z- meanExpectedMeasDiff: 0.706578 delta_rms_squared: 0.756575 alpha: 0.974246 2024-11-06T04:40:39Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:39Z- meanExpectedMeasDiff: 0.848908 delta_rms_squared: 0.727851 alpha: 0.973257 2024-11-06T04:40:39Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:39Z- meanExpectedMeasDiff: 0.273187 delta_rms_squared: 1.041314 alpha: 0.981155 2024-11-06T04:40:39Z- Subcloud NIS: 0.117416 2024-11-06T04:40:39Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:40:39Z- TNavPF:: sumWeights = 0.656366 2024-11-06T04:40:39Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:40:39Z- TNavPF::First Term of Map Covariance Matrix is 0.43 2024-11-06T04:40:39Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T04:40:39Z- TerrainNav::Resampling particle filter... 2024-11-06T04:40:39Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868038.75 sec, ping # 0. 2024-11-06T04:40:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:40:39Z- variances([0], [2], and psi_berg) = 0.132571 0.126770 0.000000 2024-11-06T04:40:39Z- publishEstimatesDecoder() - at 1730868038.75 sec 2024-11-06T04:40:39Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:40:39Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061601.19 594245.69 55.62 2024-11-06T04:40:39Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061601.09 594245.85 55.94 lat/lon: 36.70 -121.94 2024-11-06T04:40:39Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.13 0.13 0.37 0.00 2024-11-06T04:40:39Z- AHRS_M2 msg: 1730868039.11 epoch sec; seqNo:740 2024-11-06T04:40:39Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.37 psi 2024-11-06T04:40:39Z- handleDepth()- Depth_Keller msg: 1730868039.11 epoch sec; seqNo:797; depth 49.66 2024-11-06T04:40:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868039.13 epoch sec; seqNo:797; 4061657.12 north; 594241.76 east; 36.70 lat; -121.95 lon; 49.66 depth; utmZone: 10 2024-11-06T04:40:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868038.75 2024-11-06T04:40:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:39Z- waiting for 1730868042.75, time is 1730868039.13 2024-11-06T04:40:39Z- AHRS_M2 msg: 1730868039.51 epoch sec; seqNo:741 2024-11-06T04:40:39Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.37 psi 2024-11-06T04:40:39Z- handleDepth()- Depth_Keller msg: 1730868039.52 epoch sec; seqNo:798; depth 49.68 2024-11-06T04:40:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868039.53 epoch sec; seqNo:798; 4061657.48 north; 594240.54 east; 36.70 lat; -121.95 lon; 49.68 depth; utmZone: 10 2024-11-06T04:40:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868038.75 2024-11-06T04:40:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:39Z- waiting for 1730868042.75, time is 1730868039.53 2024-11-06T04:40:39Z- AHRS_M2 msg: 1730868039.91 epoch sec; seqNo:742 2024-11-06T04:40:39Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.37 psi 2024-11-06T04:40:39Z- handleDepth()- Depth_Keller msg: 1730868039.92 epoch sec; seqNo:799; depth 49.72 2024-11-06T04:40:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868039.93 epoch sec; seqNo:799; 4061657.48 north; 594240.54 east; 36.70 lat; -121.95 lon; 49.72 depth; utmZone: 10 2024-11-06T04:40:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868038.75 2024-11-06T04:40:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:40Z- waiting for 1730868042.75, time is 1730868039.93 2024-11-06T04:40:40Z- AHRS_M2 msg: 1730868040.36 epoch sec; seqNo:743 2024-11-06T04:40:40Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.37 psi 2024-11-06T04:40:40Z- handleDepth()- Depth_Keller msg: 1730868040.38 epoch sec; seqNo:800; depth 49.73 2024-11-06T04:40:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868040.41 epoch sec; seqNo:800; 4061657.86 north; 594240.53 east; 36.70 lat; -121.95 lon; 49.73 depth; utmZone: 10 2024-11-06T04:40:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868038.75 2024-11-06T04:40:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:40Z- waiting for 1730868042.75, time is 1730868040.41 2024-11-06T04:40:40Z- AHRS_M2 msg: 1730868040.73 epoch sec; seqNo:744 2024-11-06T04:40:40Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.37 psi 2024-11-06T04:40:40Z- handleDepth()- Depth_Keller msg: 1730868040.74 epoch sec; seqNo:801; depth 49.75 2024-11-06T04:40:40Z- handleDVL() - DVL msg epochMillisec: 1730868040.76 2024-11-06T04:40:40Z- handleDvl() - RDI_Pathfinder msg: 1730868040.76 sec; seqNo:281; #beams:4 2024-11-06T04:40:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.40 , 41.77 , 42.21 , 48.62 2024-11-06T04:40:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.00 2024-11-06T04:40:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868040.77 epoch sec; seqNo:801; 4061658.24 north; 594240.53 east; 36.70 lat; -121.95 lon; 49.75 depth; utmZone: 10 2024-11-06T04:40:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868038.75 2024-11-06T04:40:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:40Z- waiting for 1730868042.75, time is 1730868040.77 2024-11-06T04:40:41Z- AHRS_M2 msg: 1730868041.13 epoch sec; seqNo:745 2024-11-06T04:40:41Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.37 psi 2024-11-06T04:40:41Z- handleDepth()- Depth_Keller msg: 1730868041.13 epoch sec; seqNo:802; depth 49.78 2024-11-06T04:40:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868041.16 epoch sec; seqNo:802; 4061658.23 north; 594239.31 east; 36.70 lat; -121.95 lon; 49.78 depth; utmZone: 10 2024-11-06T04:40:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868038.75 2024-11-06T04:40:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:41Z- waiting for 1730868042.75, time is 1730868041.16 2024-11-06T04:40:41Z- AHRS_M2 msg: 1730868041.53 epoch sec; seqNo:746 2024-11-06T04:40:41Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.37 psi 2024-11-06T04:40:41Z- handleDepth()- Depth_Keller msg: 1730868041.54 epoch sec; seqNo:803; depth 49.82 2024-11-06T04:40:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868041.55 epoch sec; seqNo:803; 4061658.61 north; 594239.30 east; 36.70 lat; -121.95 lon; 49.82 depth; utmZone: 10 2024-11-06T04:40:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868038.75 2024-11-06T04:40:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:41Z- waiting for 1730868042.75, time is 1730868041.55 2024-11-06T04:40:41Z- AHRS_M2 msg: 1730868041.93 epoch sec; seqNo:747 2024-11-06T04:40:41Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.37 psi 2024-11-06T04:40:41Z- handleDepth()- Depth_Keller msg: 1730868041.94 epoch sec; seqNo:804; depth 49.83 2024-11-06T04:40:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868041.95 epoch sec; seqNo:804; 4061658.61 north; 594239.30 east; 36.70 lat; -121.95 lon; 49.83 depth; utmZone: 10 2024-11-06T04:40:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868038.75 2024-11-06T04:40:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:42Z- waiting for 1730868042.75, time is 1730868041.95 2024-11-06T04:40:42Z- AHRS_M2 msg: 1730868042.36 epoch sec; seqNo:748 2024-11-06T04:40:42Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.37 psi 2024-11-06T04:40:42Z- handleDepth()- Depth_Keller msg: 1730868042.38 epoch sec; seqNo:805; depth 49.86 2024-11-06T04:40:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868042.41 epoch sec; seqNo:805; 4061658.99 north; 594239.30 east; 36.70 lat; -121.95 lon; 49.86 depth; utmZone: 10 2024-11-06T04:40:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868038.75 2024-11-06T04:40:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:42Z- waiting for 1730868042.75, time is 1730868042.41 2024-11-06T04:40:42Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:40:42Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:40:42Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:40:42Z- AHRS_M2 msg: 1730868042.73 epoch sec; seqNo:749 2024-11-06T04:40:42Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.37 psi 2024-11-06T04:40:42Z- handleDepth()- Depth_Keller msg: 1730868042.77 epoch sec; seqNo:806; depth 49.89 2024-11-06T04:40:42Z- handleDVL() - DVL msg epochMillisec: 1730868042.79 2024-11-06T04:40:42Z- handleDvl() - RDI_Pathfinder msg: 1730868042.79 sec; seqNo:282; #beams:4 2024-11-06T04:40:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.14 , 41.41 , 42.02 , 48.14 2024-11-06T04:40:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.02 , -0.01 2024-11-06T04:40:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868042.80 epoch sec; seqNo:806; 4061659.35 north; 594238.08 east; 36.70 lat; -121.95 lon; 49.89 depth; utmZone: 10 2024-11-06T04:40:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868038.75 2024-11-06T04:40:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:42Z- waiting for 1730868042.75, time is 1730868042.80 2024-11-06T04:40:42Z- LcmTrn::updateTrn() >>>> pose time 1730868042.80, meas time 1730868042.79 <<<< 2024-11-06T04:40:42Z- LcmTrn::updateTrn() equating pose 1730868042.79 and meas times 1730868042.79 2024-11-06T04:40:42Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868042.79 2024-11-06T04:40:42Z- TerrainNav::motionUpdate() - currEstimate = (4061659, 594238.08, 49.89) 2024-11-06T04:40:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:40:42Z- TerrainNav:: SubcloudNIS is 0.12, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:40:42Z- TNavPF::Projecting Measurements 2024-11-06T04:40:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:40:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:40:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:40:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:40:42Z- TNavFilter::4 good beams. 2024-11-06T04:40:42Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:40:42Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:40:42Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:40:42Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:40:42Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:40:42Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:40:42Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:40:42Z- TerrainMapOctree: (vehN, vehE ) = (4061659.35, 594238.08). 2024-11-06T04:40:42Z- TerrainMapOctree: Min Distance = 91.43. 2024-11-06T04:40:42Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:40:43Z- Forcing Subcloud Comparison 2024-11-06T04:40:43Z- Weighting particles with subcloud comparison 2024-11-06T04:40:43Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:43Z- meanExpectedMeasDiff: -0.398651 delta_rms_squared: 0.967808 alpha: 0.979753 2024-11-06T04:40:43Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:43Z- meanExpectedMeasDiff: 0.148628 delta_rms_squared: 0.681800 alpha: 0.971502 2024-11-06T04:40:43Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:43Z- meanExpectedMeasDiff: 0.375872 delta_rms_squared: 0.709767 alpha: 0.972594 2024-11-06T04:40:43Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:43Z- meanExpectedMeasDiff: -0.027056 delta_rms_squared: 1.004531 alpha: 0.980479 2024-11-06T04:40:43Z- Subcloud NIS: 0.112299 2024-11-06T04:40:43Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:40:43Z- TNavPF:: sumWeights = 0.750911 2024-11-06T04:40:43Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:40:43Z- TNavPF::First Term of Map Covariance Matrix is 0.45 2024-11-06T04:40:43Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.01 2024-11-06T04:40:43Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868042.79 sec, ping # 0. 2024-11-06T04:40:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:40:43Z- variances([0], [2], and psi_berg) = 0.134907 0.129889 0.000000 2024-11-06T04:40:43Z- publishEstimatesDecoder() - at 1730868042.79 sec 2024-11-06T04:40:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:40:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061603.55 594241.52 56.22 2024-11-06T04:40:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061603.33 594242.16 56.20 lat/lon: 36.70 -121.95 2024-11-06T04:40:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.13 0.13 0.37 0.00 2024-11-06T04:40:43Z- AHRS_M2 msg: 1730868043.16 epoch sec; seqNo:750 2024-11-06T04:40:43Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.37 psi 2024-11-06T04:40:43Z- handleDepth()- Depth_Keller msg: 1730868043.17 epoch sec; seqNo:807; depth 49.91 2024-11-06T04:40:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868043.20 epoch sec; seqNo:807; 4061659.35 north; 594238.08 east; 36.70 lat; -121.95 lon; 49.91 depth; utmZone: 10 2024-11-06T04:40:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868042.79 2024-11-06T04:40:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:43Z- waiting for 1730868046.79, time is 1730868043.20 2024-11-06T04:40:43Z- AHRS_M2 msg: 1730868043.55 epoch sec; seqNo:751 2024-11-06T04:40:43Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.36 psi 2024-11-06T04:40:43Z- handleDepth()- Depth_Keller msg: 1730868043.56 epoch sec; seqNo:808; depth 49.94 2024-11-06T04:40:43Z- handleDVL() - DVL msg epochMillisec: 1730868043.56 2024-11-06T04:40:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:43Z- handleDvl() - RDI_Pathfinder msg: 1730868043.56 sec; seqNo:283; #beams:0 2024-11-06T04:40:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.02 , -0.01 2024-11-06T04:40:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868043.57 epoch sec; seqNo:808; 4061659.73 north; 594238.07 east; 36.70 lat; -121.95 lon; 49.94 depth; utmZone: 10 2024-11-06T04:40:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868042.79 2024-11-06T04:40:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:43Z- waiting for 1730868046.79, time is 1730868043.57 2024-11-06T04:40:43Z- AHRS_M2 msg: 1730868043.96 epoch sec; seqNo:752 2024-11-06T04:40:43Z- AHRS_M2 msg: 0.12 phi; -0.15 theta; 5.36 psi 2024-11-06T04:40:43Z- handleDepth()- Depth_Keller msg: 1730868043.96 epoch sec; seqNo:809; depth 49.97 2024-11-06T04:40:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868043.97 epoch sec; seqNo:809; 4061659.73 north; 594238.07 east; 36.70 lat; -121.95 lon; 49.97 depth; utmZone: 10 2024-11-06T04:40:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868042.79 2024-11-06T04:40:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:44Z- waiting for 1730868046.79, time is 1730868043.97 2024-11-06T04:40:44Z- AHRS_M2 msg: 1730868044.40 epoch sec; seqNo:753 2024-11-06T04:40:44Z- AHRS_M2 msg: 0.12 phi; -0.14 theta; 5.36 psi 2024-11-06T04:40:44Z- handleDepth()- Depth_Keller msg: 1730868044.41 epoch sec; seqNo:810; depth 50.01 2024-11-06T04:40:44Z- handleDVL() - DVL msg epochMillisec: 1730868044.43 2024-11-06T04:40:44Z- handleDvl() - RDI_Pathfinder msg: 1730868044.43 sec; seqNo:284; #beams:4 2024-11-06T04:40:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.94 , 41.41 , 41.82 , 47.73 2024-11-06T04:40:44Z- handleDvl() - RDI_Pathfinder msg: velocities 1.02 , -0.02 , 0.03 2024-11-06T04:40:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868044.45 epoch sec; seqNo:810; 4061660.10 north; 594236.85 east; 36.70 lat; -121.95 lon; 50.01 depth; utmZone: 10 2024-11-06T04:40:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868042.79 2024-11-06T04:40:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:44Z- waiting for 1730868046.79, time is 1730868044.45 2024-11-06T04:40:44Z- AHRS_M2 msg: 1730868044.76 epoch sec; seqNo:754 2024-11-06T04:40:44Z- AHRS_M2 msg: 0.12 phi; -0.14 theta; 5.35 psi 2024-11-06T04:40:44Z- handleDepth()- Depth_Keller msg: 1730868044.77 epoch sec; seqNo:811; depth 50.03 2024-11-06T04:40:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868044.80 epoch sec; seqNo:811; 4061660.47 north; 594236.84 east; 36.70 lat; -121.95 lon; 50.03 depth; utmZone: 10 2024-11-06T04:40:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868042.79 2024-11-06T04:40:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:44Z- waiting for 1730868046.79, time is 1730868044.80 2024-11-06T04:40:45Z- AHRS_M2 msg: 1730868045.17 epoch sec; seqNo:755 2024-11-06T04:40:45Z- AHRS_M2 msg: 0.12 phi; -0.14 theta; 5.35 psi 2024-11-06T04:40:45Z- handleDepth()- Depth_Keller msg: 1730868045.18 epoch sec; seqNo:812; depth 50.06 2024-11-06T04:40:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868045.19 epoch sec; seqNo:812; 4061660.47 north; 594236.84 east; 36.70 lat; -121.95 lon; 50.06 depth; utmZone: 10 2024-11-06T04:40:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868042.79 2024-11-06T04:40:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:45Z- waiting for 1730868046.79, time is 1730868045.19 2024-11-06T04:40:45Z- AHRS_M2 msg: 1730868045.57 epoch sec; seqNo:756 2024-11-06T04:40:45Z- AHRS_M2 msg: 0.12 phi; -0.14 theta; 5.35 psi 2024-11-06T04:40:45Z- handleDepth()- Depth_Keller msg: 1730868045.58 epoch sec; seqNo:813; depth 50.09 2024-11-06T04:40:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868045.59 epoch sec; seqNo:813; 4061660.85 north; 594236.84 east; 36.70 lat; -121.95 lon; 50.09 depth; utmZone: 10 2024-11-06T04:40:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868042.79 2024-11-06T04:40:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:45Z- waiting for 1730868046.79, time is 1730868045.59 2024-11-06T04:40:45Z- AHRS_M2 msg: 1730868045.97 epoch sec; seqNo:757 2024-11-06T04:40:45Z- AHRS_M2 msg: 0.12 phi; -0.14 theta; 5.34 psi 2024-11-06T04:40:45Z- handleDepth()- Depth_Keller msg: 1730868045.99 epoch sec; seqNo:814; depth 50.13 2024-11-06T04:40:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868046.00 epoch sec; seqNo:814; 4061660.84 north; 594235.62 east; 36.70 lat; -121.95 lon; 50.13 depth; utmZone: 10 2024-11-06T04:40:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868042.79 2024-11-06T04:40:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:46Z- waiting for 1730868046.79, time is 1730868046.00 2024-11-06T04:40:46Z- AHRS_M2 msg: 1730868046.41 epoch sec; seqNo:758 2024-11-06T04:40:46Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.34 psi 2024-11-06T04:40:46Z- handleDepth()- Depth_Keller msg: 1730868046.41 epoch sec; seqNo:815; depth 50.17 2024-11-06T04:40:46Z- handleDVL() - DVL msg epochMillisec: 1730868046.43 2024-11-06T04:40:46Z- handleDvl() - RDI_Pathfinder msg: 1730868046.43 sec; seqNo:285; #beams:4 2024-11-06T04:40:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.94 , 41.61 , 41.82 , 47.33 2024-11-06T04:40:46Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.02 , 0.08 2024-11-06T04:40:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868046.45 epoch sec; seqNo:815; 4061661.22 north; 594235.61 east; 36.70 lat; -121.95 lon; 50.17 depth; utmZone: 10 2024-11-06T04:40:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868042.79 2024-11-06T04:40:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:46Z- waiting for 1730868046.79, time is 1730868046.45 2024-11-06T04:40:46Z- AHRS_M2 msg: 1730868046.79 epoch sec; seqNo:759 2024-11-06T04:40:46Z- AHRS_M2 msg: 0.12 phi; -0.13 theta; 5.34 psi 2024-11-06T04:40:46Z- handleDepth()- Depth_Keller msg: 1730868046.80 epoch sec; seqNo:816; depth 50.23 2024-11-06T04:40:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868046.81 epoch sec; seqNo:816; 4061661.60 north; 594235.61 east; 36.70 lat; -121.95 lon; 50.23 depth; utmZone: 10 2024-11-06T04:40:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868042.79 2024-11-06T04:40:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:46Z- waiting for 1730868046.79, time is 1730868046.81 2024-11-06T04:40:46Z- LcmTrn::updateTrn() >>>> pose time 1730868046.81, meas time 1730868046.43 <<<< 2024-11-06T04:40:46Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868046.81 2024-11-06T04:40:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868046.43 sec, ping # 0; waiting for INS data more recent than 1730868042.79... 2024-11-06T04:40:46Z- TerrainNav::motionUpdate() - currEstimate = (4061662, 594235.61, 50.23) 2024-11-06T04:40:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:40:46Z- TerrainNav:: SubcloudNIS is 0.11, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:40:46Z- TNavPF::Projecting Measurements 2024-11-06T04:40:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:40:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:40:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:40:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:40:46Z- TNavFilter::4 good beams. 2024-11-06T04:40:46Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:40:46Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:40:46Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:40:46Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:40:46Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:40:46Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:40:46Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:40:46Z- TerrainMapOctree: (vehN, vehE ) = (4061661.39, 594235.84). 2024-11-06T04:40:46Z- TerrainMapOctree: Min Distance = 89.74. 2024-11-06T04:40:46Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:40:47Z- Forcing Subcloud Comparison 2024-11-06T04:40:47Z- Weighting particles with subcloud comparison 2024-11-06T04:40:47Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:47Z- meanExpectedMeasDiff: -0.064136 delta_rms_squared: 0.969916 alpha: 0.979796 2024-11-06T04:40:47Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:47Z- meanExpectedMeasDiff: -0.327960 delta_rms_squared: 0.738109 alpha: 0.973619 2024-11-06T04:40:47Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:47Z- meanExpectedMeasDiff: 0.283628 delta_rms_squared: 0.723640 alpha: 0.973105 2024-11-06T04:40:47Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:47Z- meanExpectedMeasDiff: -0.815348 delta_rms_squared: 0.533484 alpha: 0.963865 2024-11-06T04:40:47Z- Subcloud NIS: 0.111292 2024-11-06T04:40:47Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:40:47Z- TNavPF:: sumWeights = 0.729114 2024-11-06T04:40:47Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:40:47Z- TNavPF::First Term of Map Covariance Matrix is 0.46 2024-11-06T04:40:47Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T04:40:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868046.43 sec. 2024-11-06T04:40:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:40:47Z- variances([0], [2], and psi_berg) = 0.134583 0.130517 0.000000 2024-11-06T04:40:47Z- publishEstimatesDecoder() - at 1730868046.81 sec 2024-11-06T04:40:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:40:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061605.23 594240.31 56.39 2024-11-06T04:40:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061605.57 594239.71 56.60 lat/lon: 36.70 -121.95 2024-11-06T04:40:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.13 0.13 0.64 0.00 2024-11-06T04:40:47Z- AHRS_M2 msg: 1730868047.19 epoch sec; seqNo:760 2024-11-06T04:40:47Z- AHRS_M2 msg: 0.11 phi; -0.13 theta; 5.33 psi 2024-11-06T04:40:47Z- handleDepth()- Depth_Keller msg: 1730868047.21 epoch sec; seqNo:817; depth 50.25 2024-11-06T04:40:47Z- handleDVL() - DVL msg epochMillisec: 1730868047.21 2024-11-06T04:40:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:47Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:47Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:47Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:47Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:47Z- handleDvl() - RDI_Pathfinder msg: 1730868047.21 sec; seqNo:286; #beams:0 2024-11-06T04:40:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:47Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , -0.02 , 0.08 2024-11-06T04:40:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868047.22 epoch sec; seqNo:817; 4061661.58 north; 594234.39 east; 36.70 lat; -121.95 lon; 50.25 depth; utmZone: 10 2024-11-06T04:40:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868046.81 2024-11-06T04:40:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:47Z- waiting for 1730868050.81, time is 1730868047.22 2024-11-06T04:40:47Z- AHRS_M2 msg: 1730868047.59 epoch sec; seqNo:761 2024-11-06T04:40:47Z- AHRS_M2 msg: 0.11 phi; -0.13 theta; 5.33 psi 2024-11-06T04:40:47Z- handleDepth()- Depth_Keller msg: 1730868047.60 epoch sec; seqNo:818; depth 50.29 2024-11-06T04:40:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868047.61 epoch sec; seqNo:818; 4061661.96 north; 594234.39 east; 36.70 lat; -121.95 lon; 50.29 depth; utmZone: 10 2024-11-06T04:40:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868046.81 2024-11-06T04:40:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:47Z- waiting for 1730868050.81, time is 1730868047.61 2024-11-06T04:40:48Z- AHRS_M2 msg: 1730868047.99 epoch sec; seqNo:762 2024-11-06T04:40:48Z- AHRS_M2 msg: 0.11 phi; -0.13 theta; 5.33 psi 2024-11-06T04:40:48Z- handleDepth()- Depth_Keller msg: 1730868048.00 epoch sec; seqNo:819; depth 50.34 2024-11-06T04:40:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868048.01 epoch sec; seqNo:819; 4061661.96 north; 594234.39 east; 36.70 lat; -121.95 lon; 50.34 depth; utmZone: 10 2024-11-06T04:40:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868046.81 2024-11-06T04:40:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:48Z- waiting for 1730868050.81, time is 1730868048.01 2024-11-06T04:40:48Z- AHRS_M2 msg: 1730868048.41 epoch sec; seqNo:763 2024-11-06T04:40:48Z- AHRS_M2 msg: 0.11 phi; -0.12 theta; 5.32 psi 2024-11-06T04:40:48Z- handleDepth()- Depth_Keller msg: 1730868048.42 epoch sec; seqNo:820; depth 50.38 2024-11-06T04:40:48Z- handleDVL() - DVL msg epochMillisec: 1730868048.44 2024-11-06T04:40:48Z- handleDvl() - RDI_Pathfinder msg: 1730868048.44 sec; seqNo:287; #beams:4 2024-11-06T04:40:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.33 , 42.02 , 42.02 , 46.92 2024-11-06T04:40:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.01 , 0.10 2024-11-06T04:40:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868048.47 epoch sec; seqNo:820; 4061662.34 north; 594234.38 east; 36.70 lat; -121.95 lon; 50.38 depth; utmZone: 10 2024-11-06T04:40:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868046.81 2024-11-06T04:40:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:48Z- waiting for 1730868050.81, time is 1730868048.47 2024-11-06T04:40:48Z- AHRS_M2 msg: 1730868048.80 epoch sec; seqNo:764 2024-11-06T04:40:48Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.32 psi 2024-11-06T04:40:48Z- handleDepth()- Depth_Keller msg: 1730868048.81 epoch sec; seqNo:821; depth 50.43 2024-11-06T04:40:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868048.84 epoch sec; seqNo:821; 4061662.71 north; 594233.16 east; 36.70 lat; -121.95 lon; 50.43 depth; utmZone: 10 2024-11-06T04:40:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868046.81 2024-11-06T04:40:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:48Z- waiting for 1730868050.81, time is 1730868048.84 2024-11-06T04:40:49Z- AHRS_M2 msg: 1730868049.21 epoch sec; seqNo:765 2024-11-06T04:40:49Z- AHRS_M2 msg: 0.12 phi; -0.12 theta; 5.32 psi 2024-11-06T04:40:49Z- handleDepth()- Depth_Keller msg: 1730868049.22 epoch sec; seqNo:822; depth 50.45 2024-11-06T04:40:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868049.23 epoch sec; seqNo:822; 4061662.71 north; 594233.16 east; 36.70 lat; -121.95 lon; 50.45 depth; utmZone: 10 2024-11-06T04:40:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868046.81 2024-11-06T04:40:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:49Z- waiting for 1730868050.81, time is 1730868049.23 2024-11-06T04:40:49Z- handleDepth()- Depth_Keller msg: 1730868049.63 epoch sec; seqNo:823; depth 50.48 2024-11-06T04:40:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868049.65 epoch sec; seqNo:823; 4061663.09 north; 594233.15 east; 36.70 lat; -121.95 lon; 50.48 depth; utmZone: 10 2024-11-06T04:40:49Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:40:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868046.81 2024-11-06T04:40:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:49Z- waiting for 1730868050.81, time is 1730868049.65 2024-11-06T04:40:50Z- AHRS_M2 msg: 1730868050.02 epoch sec; seqNo:766 2024-11-06T04:40:50Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 5.33 psi 2024-11-06T04:40:50Z- handleDepth()- Depth_Keller msg: 1730868050.03 epoch sec; seqNo:824; depth 50.51 2024-11-06T04:40:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868050.04 epoch sec; seqNo:824; 4061663.09 north; 594233.15 east; 36.70 lat; -121.95 lon; 50.51 depth; utmZone: 10 2024-11-06T04:40:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868046.81 2024-11-06T04:40:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:50Z- waiting for 1730868050.81, time is 1730868050.04 2024-11-06T04:40:50Z- AHRS_M2 msg: 1730868050.42 epoch sec; seqNo:767 2024-11-06T04:40:50Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 5.33 psi 2024-11-06T04:40:50Z- handleDepth()- Depth_Keller msg: 1730868050.43 epoch sec; seqNo:825; depth 50.56 2024-11-06T04:40:50Z- handleDVL() - DVL msg epochMillisec: 1730868050.45 2024-11-06T04:40:50Z- handleDvl() - RDI_Pathfinder msg: 1730868050.45 sec; seqNo:288; #beams:4 2024-11-06T04:40:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.73 , 41.61 , 42.23 , 46.71 2024-11-06T04:40:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.09 2024-11-06T04:40:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868050.47 epoch sec; seqNo:825; 4061663.45 north; 594231.93 east; 36.70 lat; -121.95 lon; 50.56 depth; utmZone: 10 2024-11-06T04:40:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868046.81 2024-11-06T04:40:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:50Z- waiting for 1730868050.81, time is 1730868050.47 2024-11-06T04:40:50Z- AHRS_M2 msg: 1730868050.83 epoch sec; seqNo:768 2024-11-06T04:40:50Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 5.33 psi 2024-11-06T04:40:50Z- handleDepth()- Depth_Keller msg: 1730868050.84 epoch sec; seqNo:826; depth 50.59 2024-11-06T04:40:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868050.85 epoch sec; seqNo:826; 4061663.45 north; 594231.93 east; 36.70 lat; -121.95 lon; 50.59 depth; utmZone: 10 2024-11-06T04:40:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868046.81 2024-11-06T04:40:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:50Z- waiting for 1730868050.81, time is 1730868050.85 2024-11-06T04:40:50Z- LcmTrn::updateTrn() >>>> pose time 1730868050.85, meas time 1730868050.45 <<<< 2024-11-06T04:40:50Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868050.85 2024-11-06T04:40:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868050.45 sec, ping # 0; waiting for INS data more recent than 1730868046.81... 2024-11-06T04:40:50Z- TerrainNav::motionUpdate() - currEstimate = (4061663, 594231.93, 50.59) 2024-11-06T04:40:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:40:50Z- TerrainNav:: SubcloudNIS is 0.11, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:40:51Z- TNavPF::Projecting Measurements 2024-11-06T04:40:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:40:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:40:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:40:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:40:51Z- TNavFilter::4 good beams. 2024-11-06T04:40:51Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:40:51Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:40:51Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:40:51Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:40:51Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:40:51Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:40:51Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:40:51Z- TerrainMapOctree: (vehN, vehE ) = (4061663.27, 594232.29). 2024-11-06T04:40:51Z- TerrainMapOctree: Min Distance = 88.52. 2024-11-06T04:40:51Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:40:51Z- Forcing Subcloud Comparison 2024-11-06T04:40:51Z- Weighting particles with subcloud comparison 2024-11-06T04:40:51Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:51Z- meanExpectedMeasDiff: 0.232598 delta_rms_squared: 1.387626 alpha: 0.985792 2024-11-06T04:40:51Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:51Z- meanExpectedMeasDiff: 0.306414 delta_rms_squared: 1.046854 alpha: 0.981253 2024-11-06T04:40:51Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:51Z- meanExpectedMeasDiff: 0.737017 delta_rms_squared: 1.056213 alpha: 0.981416 2024-11-06T04:40:51Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:51Z- meanExpectedMeasDiff: -0.549784 delta_rms_squared: 1.373679 alpha: 0.985649 2024-11-06T04:40:51Z- Subcloud NIS: 0.115162 2024-11-06T04:40:51Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:40:51Z- TNavPF:: sumWeights = 0.647638 2024-11-06T04:40:51Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:40:51Z- TNavPF::First Term of Map Covariance Matrix is 0.46 2024-11-06T04:40:51Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T04:40:51Z- TerrainNav::Resampling particle filter... 2024-11-06T04:40:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868050.45 sec. 2024-11-06T04:40:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:40:51Z- variances([0], [2], and psi_berg) = 0.136835 0.132879 0.000000 2024-11-06T04:40:51Z- publishEstimatesDecoder() - at 1730868050.85 sec 2024-11-06T04:40:51Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:40:51Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061607.47 594235.94 57.34 2024-11-06T04:40:51Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061607.42 594236.03 56.87 lat/lon: 36.70 -121.95 2024-11-06T04:40:51Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.13 0.68 0.00 2024-11-06T04:40:51Z- AHRS_M2 msg: 1730868051.25 epoch sec; seqNo:769 2024-11-06T04:40:51Z- AHRS_M2 msg: 0.12 phi; -0.11 theta; 5.33 psi 2024-11-06T04:40:51Z- handleDepth()- Depth_Keller msg: 1730868051.28 epoch sec; seqNo:827; depth 50.62 2024-11-06T04:40:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868051.31 epoch sec; seqNo:827; 4061663.83 north; 594231.92 east; 36.70 lat; -121.95 lon; 50.62 depth; utmZone: 10 2024-11-06T04:40:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868050.85 2024-11-06T04:40:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:51Z- waiting for 1730868054.85, time is 1730868051.31 2024-11-06T04:40:51Z- AHRS_M2 msg: 1730868051.63 epoch sec; seqNo:770 2024-11-06T04:40:51Z- AHRS_M2 msg: 0.13 phi; -0.11 theta; 5.33 psi 2024-11-06T04:40:51Z- handleDepth()- Depth_Keller msg: 1730868051.65 epoch sec; seqNo:828; depth 50.64 2024-11-06T04:40:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868051.66 epoch sec; seqNo:828; 4061663.83 north; 594231.92 east; 36.70 lat; -121.95 lon; 50.64 depth; utmZone: 10 2024-11-06T04:40:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868050.85 2024-11-06T04:40:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:51Z- waiting for 1730868054.85, time is 1730868051.66 2024-11-06T04:40:52Z- AHRS_M2 msg: 1730868052.04 epoch sec; seqNo:771 2024-11-06T04:40:52Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 5.34 psi 2024-11-06T04:40:52Z- handleDepth()- Depth_Keller msg: 1730868052.05 epoch sec; seqNo:829; depth 50.66 2024-11-06T04:40:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868052.06 epoch sec; seqNo:829; 4061664.20 north; 594230.70 east; 36.70 lat; -121.95 lon; 50.66 depth; utmZone: 10 2024-11-06T04:40:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868050.85 2024-11-06T04:40:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:52Z- waiting for 1730868054.85, time is 1730868052.06 2024-11-06T04:40:52Z- AHRS_M2 msg: 1730868052.44 epoch sec; seqNo:772 2024-11-06T04:40:52Z- AHRS_M2 msg: 0.15 phi; -0.10 theta; 5.35 psi 2024-11-06T04:40:52Z- handleDepth()- Depth_Keller msg: 1730868052.44 epoch sec; seqNo:830; depth 50.69 2024-11-06T04:40:52Z- handleDVL() - DVL msg epochMillisec: 1730868052.46 2024-11-06T04:40:52Z- handleDvl() - RDI_Pathfinder msg: 1730868052.46 sec; seqNo:289; #beams:4 2024-11-06T04:40:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.53 , 41.41 , 42.43 , 46.51 2024-11-06T04:40:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.04 2024-11-06T04:40:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868052.50 epoch sec; seqNo:830; 4061664.20 north; 594230.70 east; 36.70 lat; -121.95 lon; 50.69 depth; utmZone: 10 2024-11-06T04:40:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868050.85 2024-11-06T04:40:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:52Z- waiting for 1730868054.85, time is 1730868052.50 2024-11-06T04:40:52Z- AHRS_M2 msg: 1730868052.85 epoch sec; seqNo:773 2024-11-06T04:40:52Z- AHRS_M2 msg: 0.17 phi; -0.10 theta; 5.36 psi 2024-11-06T04:40:52Z- handleDepth()- Depth_Keller msg: 1730868052.85 epoch sec; seqNo:831; depth 50.70 2024-11-06T04:40:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868052.87 epoch sec; seqNo:831; 4061664.57 north; 594230.70 east; 36.70 lat; -121.95 lon; 50.70 depth; utmZone: 10 2024-11-06T04:40:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868050.85 2024-11-06T04:40:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:53Z- waiting for 1730868054.85, time is 1730868052.87 2024-11-06T04:40:53Z- AHRS_M2 msg: 1730868053.26 epoch sec; seqNo:774 2024-11-06T04:40:53Z- AHRS_M2 msg: 0.19 phi; -0.10 theta; 5.37 psi 2024-11-06T04:40:53Z- handleDepth()- Depth_Keller msg: 1730868053.26 epoch sec; seqNo:832; depth 50.72 2024-11-06T04:40:53Z- handleDVL() - DVL msg epochMillisec: 1730868053.27 2024-11-06T04:40:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:53Z- handleDvl() - RDI_Pathfinder msg: 1730868053.27 sec; seqNo:290; #beams:0 2024-11-06T04:40:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.04 2024-11-06T04:40:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868053.31 epoch sec; seqNo:832; 4061664.57 north; 594230.70 east; 36.70 lat; -121.95 lon; 50.72 depth; utmZone: 10 2024-11-06T04:40:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868050.85 2024-11-06T04:40:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:53Z- waiting for 1730868054.85, time is 1730868053.31 2024-11-06T04:40:53Z- AHRS_M2 msg: 1730868053.66 epoch sec; seqNo:775 2024-11-06T04:40:53Z- AHRS_M2 msg: 0.21 phi; -0.11 theta; 5.39 psi 2024-11-06T04:40:53Z- handleDepth()- Depth_Keller msg: 1730868053.67 epoch sec; seqNo:833; depth 50.74 2024-11-06T04:40:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868053.71 epoch sec; seqNo:833; 4061664.94 north; 594229.47 east; 36.70 lat; -121.95 lon; 50.74 depth; utmZone: 10 2024-11-06T04:40:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868050.85 2024-11-06T04:40:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:53Z- waiting for 1730868054.85, time is 1730868053.71 2024-11-06T04:40:54Z- AHRS_M2 msg: 1730868054.06 epoch sec; seqNo:776 2024-11-06T04:40:54Z- AHRS_M2 msg: 0.23 phi; -0.11 theta; 5.41 psi 2024-11-06T04:40:54Z- handleDepth()- Depth_Keller msg: 1730868054.07 epoch sec; seqNo:834; depth 50.74 2024-11-06T04:40:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868054.08 epoch sec; seqNo:834; 4061664.94 north; 594229.47 east; 36.70 lat; -121.95 lon; 50.74 depth; utmZone: 10 2024-11-06T04:40:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868050.85 2024-11-06T04:40:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:54Z- waiting for 1730868054.85, time is 1730868054.08 2024-11-06T04:40:54Z- AHRS_M2 msg: 1730868054.46 epoch sec; seqNo:777 2024-11-06T04:40:54Z- AHRS_M2 msg: 0.27 phi; -0.12 theta; 5.44 psi 2024-11-06T04:40:54Z- handleDepth()- Depth_Keller msg: 1730868054.46 epoch sec; seqNo:835; depth 50.77 2024-11-06T04:40:54Z- handleDVL() - DVL msg epochMillisec: 1730868054.49 2024-11-06T04:40:54Z- handleDvl() - RDI_Pathfinder msg: 1730868054.49 sec; seqNo:291; #beams:4 2024-11-06T04:40:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 51.00 , 40.39 , 42.84 , 47.33 2024-11-06T04:40:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , -0.01 2024-11-06T04:40:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868054.51 epoch sec; seqNo:835; 4061665.32 north; 594229.47 east; 36.70 lat; -121.95 lon; 50.77 depth; utmZone: 10 2024-11-06T04:40:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868050.85 2024-11-06T04:40:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:54Z- waiting for 1730868054.85, time is 1730868054.51 2024-11-06T04:40:54Z- AHRS_M2 msg: 1730868054.86 epoch sec; seqNo:778 2024-11-06T04:40:54Z- AHRS_M2 msg: 0.30 phi; -0.12 theta; 5.47 psi 2024-11-06T04:40:54Z- handleDepth()- Depth_Keller msg: 1730868054.87 epoch sec; seqNo:836; depth 50.79 2024-11-06T04:40:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868054.88 epoch sec; seqNo:836; 4061665.70 north; 594229.46 east; 36.70 lat; -121.95 lon; 50.79 depth; utmZone: 10 2024-11-06T04:40:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868050.85 2024-11-06T04:40:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:55Z- waiting for 1730868054.85, time is 1730868054.88 2024-11-06T04:40:55Z- LcmTrn::updateTrn() >>>> pose time 1730868054.88, meas time 1730868054.49 <<<< 2024-11-06T04:40:55Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868054.88 2024-11-06T04:40:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868054.49 sec, ping # 0; waiting for INS data more recent than 1730868050.85... 2024-11-06T04:40:55Z- TerrainNav::motionUpdate() - currEstimate = (4061666, 594229.46, 50.79) 2024-11-06T04:40:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:40:55Z- TerrainNav:: SubcloudNIS is 0.12, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:40:55Z- TNavPF::Projecting Measurements 2024-11-06T04:40:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:40:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:40:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:40:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:40:55Z- TNavFilter::4 good beams. 2024-11-06T04:40:55Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:40:55Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:40:55Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:40:55Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:40:55Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:40:55Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:40:55Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:40:55Z- TerrainMapOctree: (vehN, vehE ) = (4061665.48, 594229.70). 2024-11-06T04:40:55Z- TerrainMapOctree: Min Distance = 86.92. 2024-11-06T04:40:55Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:40:55Z- Forcing Subcloud Comparison 2024-11-06T04:40:55Z- Weighting particles with subcloud comparison 2024-11-06T04:40:55Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:55Z- meanExpectedMeasDiff: -3.857769 delta_rms_squared: 1.731547 alpha: 0.988582 2024-11-06T04:40:55Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:55Z- meanExpectedMeasDiff: 1.368889 delta_rms_squared: 1.002960 alpha: 0.980449 2024-11-06T04:40:55Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:55Z- meanExpectedMeasDiff: -0.796813 delta_rms_squared: 1.192669 alpha: 0.983507 2024-11-06T04:40:55Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:55Z- meanExpectedMeasDiff: -1.746288 delta_rms_squared: 1.648845 alpha: 0.988016 2024-11-06T04:40:55Z- Subcloud NIS: 0.137702 2024-11-06T04:40:55Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:40:55Z- TNavPF:: sumWeights = 0.154669 2024-11-06T04:40:55Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:40:55Z- TNavPF::First Term of Map Covariance Matrix is 1.23 2024-11-06T04:40:55Z- TNavPF::Calculated NIS Value : 0.07 numBeams normalized NIS: 0.02 2024-11-06T04:40:55Z- TerrainNav::Resampling particle filter... 2024-11-06T04:40:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868054.49 sec. 2024-11-06T04:40:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:40:55Z- variances([0], [2], and psi_berg) = 0.141014 0.123946 0.000000 2024-11-06T04:40:55Z- publishEstimatesDecoder() - at 1730868054.88 sec 2024-11-06T04:40:55Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:40:55Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061609.77 594233.47 57.22 2024-11-06T04:40:55Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061609.68 594233.52 57.58 lat/lon: 36.70 -121.95 2024-11-06T04:40:55Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.12 0.70 0.00 2024-11-06T04:40:55Z- handleDepth()- Depth_Keller msg: 1730868055.30 epoch sec; seqNo:837; depth 50.80 2024-11-06T04:40:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868055.34 epoch sec; seqNo:837; 4061665.70 north; 594229.46 east; 36.70 lat; -121.95 lon; 50.80 depth; utmZone: 10 2024-11-06T04:40:55Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:40:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868054.88 2024-11-06T04:40:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:55Z- waiting for 1730868058.88, time is 1730868055.34 2024-11-06T04:40:55Z- AHRS_M2 msg: 1730868055.68 epoch sec; seqNo:779 2024-11-06T04:40:55Z- AHRS_M2 msg: 0.36 phi; -0.13 theta; 5.53 psi 2024-11-06T04:40:55Z- handleDepth()- Depth_Keller msg: 1730868055.69 epoch sec; seqNo:838; depth 50.81 2024-11-06T04:40:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868055.71 epoch sec; seqNo:838; 4061666.06 north; 594228.24 east; 36.70 lat; -121.95 lon; 50.81 depth; utmZone: 10 2024-11-06T04:40:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868054.88 2024-11-06T04:40:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:55Z- waiting for 1730868058.88, time is 1730868055.71 2024-11-06T04:40:56Z- AHRS_M2 msg: 1730868056.08 epoch sec; seqNo:780 2024-11-06T04:40:56Z- AHRS_M2 msg: 0.39 phi; -0.14 theta; 5.57 psi 2024-11-06T04:40:56Z- handleDepth()- Depth_Keller msg: 1730868056.09 epoch sec; seqNo:839; depth 50.82 2024-11-06T04:40:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868056.11 epoch sec; seqNo:839; 4061666.44 north; 594228.23 east; 36.70 lat; -121.95 lon; 50.82 depth; utmZone: 10 2024-11-06T04:40:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868054.88 2024-11-06T04:40:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:56Z- waiting for 1730868058.88, time is 1730868056.11 2024-11-06T04:40:56Z- AHRS_M2 msg: 1730868056.48 epoch sec; seqNo:781 2024-11-06T04:40:56Z- AHRS_M2 msg: 0.41 phi; -0.15 theta; 5.61 psi 2024-11-06T04:40:56Z- handleDepth()- Depth_Keller msg: 1730868056.49 epoch sec; seqNo:840; depth 50.85 2024-11-06T04:40:56Z- handleDVL() - DVL msg epochMillisec: 1730868056.51 2024-11-06T04:40:56Z- handleDvl() - RDI_Pathfinder msg: 1730868056.51 sec; seqNo:292; #beams:4 2024-11-06T04:40:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 59.88 , 39.11 , 43.97 , 50.82 2024-11-06T04:40:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , -0.04 2024-11-06T04:40:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868056.53 epoch sec; seqNo:840; 4061666.44 north; 594228.23 east; 36.70 lat; -121.95 lon; 50.85 depth; utmZone: 10 2024-11-06T04:40:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868054.88 2024-11-06T04:40:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:56Z- waiting for 1730868058.88, time is 1730868056.53 2024-11-06T04:40:56Z- AHRS_M2 msg: 1730868056.89 epoch sec; seqNo:782 2024-11-06T04:40:56Z- AHRS_M2 msg: 0.43 phi; -0.16 theta; 5.64 psi 2024-11-06T04:40:56Z- handleDepth()- Depth_Keller msg: 1730868056.91 epoch sec; seqNo:841; depth 50.88 2024-11-06T04:40:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868056.92 epoch sec; seqNo:841; 4061666.82 north; 594228.23 east; 36.70 lat; -121.95 lon; 50.88 depth; utmZone: 10 2024-11-06T04:40:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868054.88 2024-11-06T04:40:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:57Z- waiting for 1730868058.88, time is 1730868056.92 2024-11-06T04:40:57Z- AHRS_M2 msg: 1730868057.30 epoch sec; seqNo:783 2024-11-06T04:40:57Z- AHRS_M2 msg: 0.43 phi; -0.16 theta; 5.68 psi 2024-11-06T04:40:57Z- handleDepth()- Depth_Keller msg: 1730868057.30 epoch sec; seqNo:842; depth 50.90 2024-11-06T04:40:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868057.31 epoch sec; seqNo:842; 4061667.20 north; 594228.23 east; 36.70 lat; -121.95 lon; 50.90 depth; utmZone: 10 2024-11-06T04:40:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868054.88 2024-11-06T04:40:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:57Z- waiting for 1730868058.88, time is 1730868057.31 2024-11-06T04:40:57Z- AHRS_M2 msg: 1730868057.71 epoch sec; seqNo:784 2024-11-06T04:40:57Z- AHRS_M2 msg: 0.43 phi; -0.17 theta; 5.72 psi 2024-11-06T04:40:57Z- handleDepth()- Depth_Keller msg: 1730868057.71 epoch sec; seqNo:843; depth 50.95 2024-11-06T04:40:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868057.73 epoch sec; seqNo:843; 4061667.57 north; 594227.00 east; 36.70 lat; -121.95 lon; 50.95 depth; utmZone: 10 2024-11-06T04:40:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868054.88 2024-11-06T04:40:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:57Z- waiting for 1730868058.88, time is 1730868057.73 2024-11-06T04:40:58Z- AHRS_M2 msg: 1730868058.11 epoch sec; seqNo:785 2024-11-06T04:40:58Z- AHRS_M2 msg: 0.43 phi; -0.17 theta; 5.76 psi 2024-11-06T04:40:58Z- handleDepth()- Depth_Keller msg: 1730868058.12 epoch sec; seqNo:844; depth 50.97 2024-11-06T04:40:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868058.13 epoch sec; seqNo:844; 4061667.94 north; 594227.00 east; 36.70 lat; -121.95 lon; 50.97 depth; utmZone: 10 2024-11-06T04:40:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868054.88 2024-11-06T04:40:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:58Z- waiting for 1730868058.88, time is 1730868058.13 2024-11-06T04:40:58Z- AHRS_M2 msg: 1730868058.51 epoch sec; seqNo:786 2024-11-06T04:40:58Z- AHRS_M2 msg: 0.43 phi; -0.17 theta; 5.79 psi 2024-11-06T04:40:58Z- handleDepth()- Depth_Keller msg: 1730868058.51 epoch sec; seqNo:845; depth 51.01 2024-11-06T04:40:58Z- handleDVL() - DVL msg epochMillisec: 1730868058.55 2024-11-06T04:40:58Z- handleDvl() - RDI_Pathfinder msg: 1730868058.55 sec; seqNo:293; #beams:4 2024-11-06T04:40:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.18 , 37.56 , 44.41 , 53.25 2024-11-06T04:40:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.08 , -0.03 2024-11-06T04:40:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868058.56 epoch sec; seqNo:845; 4061667.94 north; 594227.00 east; 36.70 lat; -121.95 lon; 51.01 depth; utmZone: 10 2024-11-06T04:40:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868054.88 2024-11-06T04:40:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:58Z- waiting for 1730868058.88, time is 1730868058.56 2024-11-06T04:40:58Z- AHRS_M2 msg: 1730868058.91 epoch sec; seqNo:787 2024-11-06T04:40:58Z- AHRS_M2 msg: 0.42 phi; -0.17 theta; 5.83 psi 2024-11-06T04:40:58Z- handleDepth()- Depth_Keller msg: 1730868058.92 epoch sec; seqNo:846; depth 51.05 2024-11-06T04:40:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868058.94 epoch sec; seqNo:846; 4061668.32 north; 594226.99 east; 36.70 lat; -121.95 lon; 51.05 depth; utmZone: 10 2024-11-06T04:40:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:40:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868054.88 2024-11-06T04:40:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:59Z- waiting for 1730868058.88, time is 1730868058.94 2024-11-06T04:40:59Z- LcmTrn::updateTrn() >>>> pose time 1730868058.94, meas time 1730868058.55 <<<< 2024-11-06T04:40:59Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868058.94 2024-11-06T04:40:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868058.55 sec, ping # 0; waiting for INS data more recent than 1730868054.88... 2024-11-06T04:40:59Z- TerrainNav::motionUpdate() - currEstimate = (4061668, 594226.99, 51.05) 2024-11-06T04:40:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:40:59Z- TerrainNav:: SubcloudNIS is 0.14, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:40:59Z- TNavPF::Projecting Measurements 2024-11-06T04:40:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:40:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:40:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:40:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:40:59Z- TNavFilter::4 good beams. 2024-11-06T04:40:59Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:40:59Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:40:59Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:40:59Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:40:59Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:40:59Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:40:59Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:40:59Z- TerrainMapOctree: (vehN, vehE ) = (4061668.07, 594227.23). 2024-11-06T04:40:59Z- TerrainMapOctree: Min Distance = 85.04. 2024-11-06T04:40:59Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:40:59Z- Forcing Subcloud Comparison 2024-11-06T04:40:59Z- Weighting particles with subcloud comparison 2024-11-06T04:40:59Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:59Z- meanExpectedMeasDiff: 2.383780 delta_rms_squared: 2.773010 alpha: 0.992839 2024-11-06T04:40:59Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:59Z- meanExpectedMeasDiff: -0.057606 delta_rms_squared: 0.990326 alpha: 0.980204 2024-11-06T04:40:59Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:59Z- meanExpectedMeasDiff: 0.727005 delta_rms_squared: 1.268551 alpha: 0.984479 2024-11-06T04:40:59Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:40:59Z- meanExpectedMeasDiff: 1.228091 delta_rms_squared: 1.061194 alpha: 0.981502 2024-11-06T04:40:59Z- Subcloud NIS: 0.141018 2024-11-06T04:40:59Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:40:59Z- TNavPF:: sumWeights = 0.503104 2024-11-06T04:40:59Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:40:59Z- TNavPF::First Term of Map Covariance Matrix is 1.08 2024-11-06T04:40:59Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T04:40:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868058.55 sec. 2024-11-06T04:40:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:40:59Z- variances([0], [2], and psi_berg) = 0.141122 0.123970 0.000000 2024-11-06T04:40:59Z- publishEstimatesDecoder() - at 1730868058.94 sec 2024-11-06T04:40:59Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:40:59Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061612.66 594230.78 56.83 2024-11-06T04:40:59Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061612.31 594231.04 57.47 lat/lon: 36.70 -121.95 2024-11-06T04:40:59Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.12 0.73 0.00 2024-11-06T04:40:59Z- AHRS_M2 msg: 1730868059.35 epoch sec; seqNo:788 2024-11-06T04:40:59Z- AHRS_M2 msg: 0.41 phi; -0.17 theta; 5.90 psi 2024-11-06T04:40:59Z- handleDepth()- Depth_Keller msg: 1730868059.38 epoch sec; seqNo:847; depth 51.11 2024-11-06T04:40:59Z- handleDVL() - DVL msg epochMillisec: 1730868059.38 2024-11-06T04:40:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:40:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:40:59Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:40:59Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:40:59Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:40:59Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:40:59Z- handleDvl() - RDI_Pathfinder msg: 1730868059.38 sec; seqNo:294; #beams:0 2024-11-06T04:40:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:40:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.08 , -0.03 2024-11-06T04:40:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868059.40 epoch sec; seqNo:847; 4061668.70 north; 594226.99 east; 36.70 lat; -121.95 lon; 51.11 depth; utmZone: 10 2024-11-06T04:40:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:40:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868058.94 2024-11-06T04:40:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:59Z- waiting for 1730868062.94, time is 1730868059.40 2024-11-06T04:40:59Z- handleDepth()- Depth_Keller msg: 1730868059.73 epoch sec; seqNo:848; depth 51.15 2024-11-06T04:40:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868059.74 epoch sec; seqNo:848; 4061669.08 north; 594226.98 east; 36.70 lat; -121.95 lon; 51.15 depth; utmZone: 10 2024-11-06T04:40:59Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:40:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868058.94 2024-11-06T04:40:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:40:59Z- waiting for 1730868062.94, time is 1730868059.74 2024-11-06T04:41:00Z- AHRS_M2 msg: 1730868060.13 epoch sec; seqNo:789 2024-11-06T04:41:00Z- AHRS_M2 msg: 0.41 phi; -0.17 theta; 5.96 psi 2024-11-06T04:41:00Z- handleDepth()- Depth_Keller msg: 1730868060.14 epoch sec; seqNo:849; depth 51.21 2024-11-06T04:41:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868060.16 epoch sec; seqNo:849; 4061669.46 north; 594226.98 east; 36.70 lat; -121.95 lon; 51.21 depth; utmZone: 10 2024-11-06T04:41:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868058.94 2024-11-06T04:41:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:00Z- waiting for 1730868062.94, time is 1730868060.16 2024-11-06T04:41:00Z- AHRS_M2 msg: 1730868060.53 epoch sec; seqNo:790 2024-11-06T04:41:00Z- AHRS_M2 msg: 0.40 phi; -0.17 theta; 6.00 psi 2024-11-06T04:41:00Z- handleDepth()- Depth_Keller msg: 1730868060.53 epoch sec; seqNo:850; depth 51.26 2024-11-06T04:41:00Z- handleDVL() - DVL msg epochMillisec: 1730868060.55 2024-11-06T04:41:00Z- handleDvl() - RDI_Pathfinder msg: 1730868060.55 sec; seqNo:295; #beams:4 2024-11-06T04:41:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.01 , 38.55 , 43.55 , 53.30 2024-11-06T04:41:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.12 , 0.01 2024-11-06T04:41:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868060.58 epoch sec; seqNo:850; 4061669.82 north; 594225.76 east; 36.70 lat; -121.95 lon; 51.26 depth; utmZone: 10 2024-11-06T04:41:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868058.94 2024-11-06T04:41:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:00Z- waiting for 1730868062.94, time is 1730868060.58 2024-11-06T04:41:00Z- AHRS_M2 msg: 1730868060.93 epoch sec; seqNo:791 2024-11-06T04:41:00Z- AHRS_M2 msg: 0.40 phi; -0.17 theta; 6.04 psi 2024-11-06T04:41:00Z- handleDepth()- Depth_Keller msg: 1730868060.94 epoch sec; seqNo:851; depth 51.31 2024-11-06T04:41:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868060.95 epoch sec; seqNo:851; 4061670.20 north; 594225.75 east; 36.70 lat; -121.95 lon; 51.31 depth; utmZone: 10 2024-11-06T04:41:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868058.94 2024-11-06T04:41:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:01Z- waiting for 1730868062.94, time is 1730868060.95 2024-11-06T04:41:01Z- AHRS_M2 msg: 1730868061.33 epoch sec; seqNo:792 2024-11-06T04:41:01Z- AHRS_M2 msg: 0.40 phi; -0.17 theta; 6.07 psi 2024-11-06T04:41:01Z- handleDepth()- Depth_Keller msg: 1730868061.34 epoch sec; seqNo:852; depth 51.37 2024-11-06T04:41:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868061.36 epoch sec; seqNo:852; 4061670.58 north; 594225.75 east; 36.70 lat; -121.95 lon; 51.37 depth; utmZone: 10 2024-11-06T04:41:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868058.94 2024-11-06T04:41:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:01Z- waiting for 1730868062.94, time is 1730868061.36 2024-11-06T04:41:01Z- AHRS_M2 msg: 1730868061.73 epoch sec; seqNo:793 2024-11-06T04:41:01Z- AHRS_M2 msg: 0.40 phi; -0.17 theta; 6.11 psi 2024-11-06T04:41:01Z- handleDepth()- Depth_Keller msg: 1730868061.74 epoch sec; seqNo:853; depth 51.42 2024-11-06T04:41:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868061.75 epoch sec; seqNo:853; 4061670.96 north; 594225.74 east; 36.70 lat; -121.95 lon; 51.42 depth; utmZone: 10 2024-11-06T04:41:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868058.94 2024-11-06T04:41:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:01Z- waiting for 1730868062.94, time is 1730868061.75 2024-11-06T04:41:02Z- handleDepth()- Depth_Keller msg: 1730868062.16 epoch sec; seqNo:854; depth 51.48 2024-11-06T04:41:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868062.17 epoch sec; seqNo:854; 4061671.34 north; 594225.74 east; 36.70 lat; -121.95 lon; 51.48 depth; utmZone: 10 2024-11-06T04:41:02Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:41:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868058.94 2024-11-06T04:41:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:02Z- waiting for 1730868062.94, time is 1730868062.17 2024-11-06T04:41:02Z- handleDepth()- Depth_Keller msg: 1730868062.57 epoch sec; seqNo:855; depth 51.55 2024-11-06T04:41:02Z- handleDVL() - DVL msg epochMillisec: 1730868062.59 2024-11-06T04:41:02Z- handleDvl() - RDI_Pathfinder msg: 1730868062.59 sec; seqNo:296; #beams:4 2024-11-06T04:41:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.54 , 38.55 , 43.55 , 52.11 2024-11-06T04:41:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.09 , 0.04 2024-11-06T04:41:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868062.62 epoch sec; seqNo:855; 4061671.72 north; 594225.73 east; 36.70 lat; -121.95 lon; 51.55 depth; utmZone: 10 2024-11-06T04:41:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868058.94 2024-11-06T04:41:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:02Z- waiting for 1730868062.94, time is 1730868062.62 2024-11-06T04:41:02Z- AHRS_M2 msg: 1730868062.95 epoch sec; seqNo:794 2024-11-06T04:41:02Z- AHRS_M2 msg: 0.40 phi; -0.15 theta; 6.22 psi 2024-11-06T04:41:02Z- handleDepth()- Depth_Keller msg: 1730868062.96 epoch sec; seqNo:856; depth 51.62 2024-11-06T04:41:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868062.97 epoch sec; seqNo:856; 4061671.72 north; 594225.73 east; 36.70 lat; -121.95 lon; 51.62 depth; utmZone: 10 2024-11-06T04:41:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868058.94 2024-11-06T04:41:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:03Z- waiting for 1730868062.94, time is 1730868062.97 2024-11-06T04:41:03Z- LcmTrn::updateTrn() >>>> pose time 1730868062.97, meas time 1730868062.59 <<<< 2024-11-06T04:41:03Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868062.97 2024-11-06T04:41:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868062.59 sec, ping # 0; waiting for INS data more recent than 1730868058.94... 2024-11-06T04:41:03Z- TerrainNav::motionUpdate() - currEstimate = (4061672, 594225.73, 51.62) 2024-11-06T04:41:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:41:03Z- TerrainNav:: SubcloudNIS is 0.14, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:41:03Z- TNavPF::Projecting Measurements 2024-11-06T04:41:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:41:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:41:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:41:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:41:03Z- TNavFilter::4 good beams. 2024-11-06T04:41:03Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:41:03Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:41:03Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:41:03Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:41:03Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:41:03Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:41:03Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:41:03Z- TerrainMapOctree: (vehN, vehE ) = (4061671.40, 594225.85). 2024-11-06T04:41:03Z- TerrainMapOctree: Min Distance = 82.22. 2024-11-06T04:41:03Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:41:03Z- Forcing Subcloud Comparison 2024-11-06T04:41:03Z- Weighting particles with subcloud comparison 2024-11-06T04:41:03Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:03Z- meanExpectedMeasDiff: 0.210025 delta_rms_squared: 2.736720 alpha: 0.992745 2024-11-06T04:41:03Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:03Z- meanExpectedMeasDiff: 1.106386 delta_rms_squared: 1.003337 alpha: 0.980456 2024-11-06T04:41:03Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:03Z- meanExpectedMeasDiff: -0.005449 delta_rms_squared: 1.289200 alpha: 0.984723 2024-11-06T04:41:03Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:03Z- meanExpectedMeasDiff: -0.894578 delta_rms_squared: 2.480760 alpha: 0.992002 2024-11-06T04:41:03Z- Subcloud NIS: 0.143793 2024-11-06T04:41:03Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:41:03Z- TNavPF:: sumWeights = 0.541137 2024-11-06T04:41:03Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:41:03Z- TNavPF::First Term of Map Covariance Matrix is 0.67 2024-11-06T04:41:03Z- TNavPF::Calculated NIS Value : 0.19 numBeams normalized NIS: 0.05 2024-11-06T04:41:03Z- TerrainNav::Resampling particle filter... 2024-11-06T04:41:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868062.59 sec. 2024-11-06T04:41:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:41:03Z- variances([0], [2], and psi_berg) = 0.141530 0.127200 0.000000 2024-11-06T04:41:03Z- publishEstimatesDecoder() - at 1730868062.97 sec 2024-11-06T04:41:03Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:41:03Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061615.83 594229.78 58.21 2024-11-06T04:41:03Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061615.71 594229.79 57.98 lat/lon: 36.70 -121.95 2024-11-06T04:41:03Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.13 0.70 0.00 2024-11-06T04:41:03Z- AHRS_M2 msg: 1730868063.38 epoch sec; seqNo:795 2024-11-06T04:41:03Z- AHRS_M2 msg: 0.40 phi; -0.15 theta; 6.26 psi 2024-11-06T04:41:03Z- handleDepth()- Depth_Keller msg: 1730868063.40 epoch sec; seqNo:857; depth 51.67 2024-11-06T04:41:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868063.42 epoch sec; seqNo:857; 4061672.10 north; 594225.73 east; 36.70 lat; -121.95 lon; 51.67 depth; utmZone: 10 2024-11-06T04:41:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868062.97 2024-11-06T04:41:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:03Z- waiting for 1730868066.97, time is 1730868063.42 2024-11-06T04:41:03Z- AHRS_M2 msg: 1730868063.76 epoch sec; seqNo:796 2024-11-06T04:41:03Z- AHRS_M2 msg: 0.39 phi; -0.14 theta; 0.01 psi 2024-11-06T04:41:03Z- handleDepth()- Depth_Keller msg: 1730868063.77 epoch sec; seqNo:858; depth 51.72 2024-11-06T04:41:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868063.78 epoch sec; seqNo:858; 4061672.48 north; 594225.73 east; 36.70 lat; -121.95 lon; 51.72 depth; utmZone: 10 2024-11-06T04:41:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868062.97 2024-11-06T04:41:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:03Z- waiting for 1730868066.97, time is 1730868063.78 2024-11-06T04:41:04Z- AHRS_M2 msg: 1730868064.16 epoch sec; seqNo:797 2024-11-06T04:41:04Z- AHRS_M2 msg: 0.39 phi; -0.13 theta; 0.05 psi 2024-11-06T04:41:04Z- handleDepth()- Depth_Keller msg: 1730868064.17 epoch sec; seqNo:859; depth 51.78 2024-11-06T04:41:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868064.19 epoch sec; seqNo:859; 4061672.86 north; 594225.72 east; 36.70 lat; -121.95 lon; 51.78 depth; utmZone: 10 2024-11-06T04:41:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868062.97 2024-11-06T04:41:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:04Z- waiting for 1730868066.97, time is 1730868064.19 2024-11-06T04:41:04Z- AHRS_M2 msg: 1730868064.57 epoch sec; seqNo:798 2024-11-06T04:41:04Z- AHRS_M2 msg: 0.39 phi; -0.12 theta; 0.09 psi 2024-11-06T04:41:04Z- handleDepth()- Depth_Keller msg: 1730868064.57 epoch sec; seqNo:860; depth 51.83 2024-11-06T04:41:04Z- handleDVL() - DVL msg epochMillisec: 1730868064.60 2024-11-06T04:41:04Z- handleDvl() - RDI_Pathfinder msg: 1730868064.60 sec; seqNo:297; #beams:4 2024-11-06T04:41:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.06 , 36.65 , 43.78 , 51.16 2024-11-06T04:41:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.03 , 0.03 2024-11-06T04:41:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868064.61 epoch sec; seqNo:860; 4061673.23 north; 594225.72 east; 36.70 lat; -121.95 lon; 51.83 depth; utmZone: 10 2024-11-06T04:41:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868062.97 2024-11-06T04:41:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:04Z- waiting for 1730868066.97, time is 1730868064.61 2024-11-06T04:41:04Z- AHRS_M2 msg: 1730868064.97 epoch sec; seqNo:799 2024-11-06T04:41:04Z- AHRS_M2 msg: 0.39 phi; -0.11 theta; 0.12 psi 2024-11-06T04:41:04Z- handleDepth()- Depth_Keller msg: 1730868064.98 epoch sec; seqNo:861; depth 51.87 2024-11-06T04:41:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868064.99 epoch sec; seqNo:861; 4061673.61 north; 594225.71 east; 36.70 lat; -121.95 lon; 51.87 depth; utmZone: 10 2024-11-06T04:41:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868062.97 2024-11-06T04:41:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:05Z- waiting for 1730868066.97, time is 1730868064.99 2024-11-06T04:41:05Z- AHRS_M2 msg: 1730868065.38 epoch sec; seqNo:800 2024-11-06T04:41:05Z- AHRS_M2 msg: 0.39 phi; -0.10 theta; 0.16 psi 2024-11-06T04:41:05Z- handleDepth()- Depth_Keller msg: 1730868065.39 epoch sec; seqNo:862; depth 51.94 2024-11-06T04:41:05Z- handleDVL() - DVL msg epochMillisec: 1730868065.39 2024-11-06T04:41:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:41:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:41:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:41:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:41:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:41:05Z- handleDvl() - RDI_Pathfinder msg: 1730868065.39 sec; seqNo:298; #beams:0 2024-11-06T04:41:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:41:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.03 , 0.03 2024-11-06T04:41:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868065.40 epoch sec; seqNo:862; 4061673.61 north; 594225.71 east; 36.70 lat; -121.95 lon; 51.94 depth; utmZone: 10 2024-11-06T04:41:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868062.97 2024-11-06T04:41:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:05Z- waiting for 1730868066.97, time is 1730868065.40 2024-11-06T04:41:05Z- AHRS_M2 msg: 1730868065.78 epoch sec; seqNo:801 2024-11-06T04:41:05Z- AHRS_M2 msg: 0.39 phi; -0.09 theta; 0.20 psi 2024-11-06T04:41:05Z- handleDepth()- Depth_Keller msg: 1730868065.80 epoch sec; seqNo:863; depth 51.97 2024-11-06T04:41:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868065.82 epoch sec; seqNo:863; 4061673.99 north; 594225.71 east; 36.70 lat; -121.95 lon; 51.97 depth; utmZone: 10 2024-11-06T04:41:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868062.97 2024-11-06T04:41:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:05Z- waiting for 1730868066.97, time is 1730868065.82 2024-11-06T04:41:06Z- AHRS_M2 msg: 1730868066.18 epoch sec; seqNo:802 2024-11-06T04:41:06Z- AHRS_M2 msg: 0.40 phi; -0.09 theta; 0.23 psi 2024-11-06T04:41:06Z- handleDepth()- Depth_Keller msg: 1730868066.19 epoch sec; seqNo:864; depth 52.00 2024-11-06T04:41:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868066.20 epoch sec; seqNo:864; 4061674.37 north; 594225.71 east; 36.70 lat; -121.95 lon; 52.00 depth; utmZone: 10 2024-11-06T04:41:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868062.97 2024-11-06T04:41:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:06Z- waiting for 1730868066.97, time is 1730868066.20 2024-11-06T04:41:06Z- AHRS_M2 msg: 1730868066.59 epoch sec; seqNo:803 2024-11-06T04:41:06Z- AHRS_M2 msg: 0.40 phi; -0.08 theta; 0.27 psi 2024-11-06T04:41:06Z- handleDepth()- Depth_Keller msg: 1730868066.59 epoch sec; seqNo:865; depth 52.02 2024-11-06T04:41:06Z- handleDVL() - DVL msg epochMillisec: 1730868066.62 2024-11-06T04:41:06Z- handleDvl() - RDI_Pathfinder msg: 1730868066.62 sec; seqNo:299; #beams:4 2024-11-06T04:41:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.65 , 38.23 , 44.19 , 49.50 2024-11-06T04:41:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.00 , -0.01 2024-11-06T04:41:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868066.69 epoch sec; seqNo:865; 4061674.75 north; 594225.70 east; 36.70 lat; -121.95 lon; 52.02 depth; utmZone: 10 2024-11-06T04:41:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868062.97 2024-11-06T04:41:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:06Z- waiting for 1730868066.97, time is 1730868066.69 2024-11-06T04:41:07Z- AHRS_M2 msg: 1730868066.99 epoch sec; seqNo:804 2024-11-06T04:41:07Z- AHRS_M2 msg: 0.39 phi; -0.08 theta; 0.30 psi 2024-11-06T04:41:07Z- handleDepth()- Depth_Keller msg: 1730868067.00 epoch sec; seqNo:866; depth 52.06 2024-11-06T04:41:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868067.02 epoch sec; seqNo:866; 4061674.75 north; 594225.70 east; 36.70 lat; -121.95 lon; 52.06 depth; utmZone: 10 2024-11-06T04:41:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868062.97 2024-11-06T04:41:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:07Z- waiting for 1730868066.97, time is 1730868067.02 2024-11-06T04:41:07Z- LcmTrn::updateTrn() >>>> pose time 1730868067.02, meas time 1730868066.62 <<<< 2024-11-06T04:41:07Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868067.02 2024-11-06T04:41:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868066.62 sec, ping # 0; waiting for INS data more recent than 1730868062.97... 2024-11-06T04:41:07Z- TerrainNav::motionUpdate() - currEstimate = (4061675, 594225.70, 52.06) 2024-11-06T04:41:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:41:07Z- TerrainNav:: SubcloudNIS is 0.14, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:41:07Z- TNavPF::Projecting Measurements 2024-11-06T04:41:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:41:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:41:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:41:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:41:07Z- TNavFilter::4 good beams. 2024-11-06T04:41:07Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:41:07Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:41:07Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:41:07Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:41:07Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:41:07Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:41:07Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:41:07Z- TerrainMapOctree: (vehN, vehE ) = (4061674.45, 594225.70). 2024-11-06T04:41:07Z- TerrainMapOctree: Min Distance = 79.36. 2024-11-06T04:41:07Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:41:07Z- Forcing Subcloud Comparison 2024-11-06T04:41:07Z- Weighting particles with subcloud comparison 2024-11-06T04:41:07Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:07Z- meanExpectedMeasDiff: 0.246302 delta_rms_squared: 2.559005 alpha: 0.992245 2024-11-06T04:41:07Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:07Z- meanExpectedMeasDiff: 1.470337 delta_rms_squared: 0.961956 alpha: 0.979632 2024-11-06T04:41:07Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:07Z- meanExpectedMeasDiff: 2.114188 delta_rms_squared: 1.290067 alpha: 0.984734 2024-11-06T04:41:07Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:07Z- meanExpectedMeasDiff: 0.151501 delta_rms_squared: 1.649027 alpha: 0.988017 2024-11-06T04:41:07Z- Subcloud NIS: 0.153226 2024-11-06T04:41:07Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:41:07Z- TNavPF:: sumWeights = 0.409993 2024-11-06T04:41:07Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:41:07Z- TNavPF::First Term of Map Covariance Matrix is 0.52 2024-11-06T04:41:07Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.05 2024-11-06T04:41:07Z- TerrainNav::Resampling particle filter... 2024-11-06T04:41:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868066.62 sec. 2024-11-06T04:41:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:41:07Z- variances([0], [2], and psi_berg) = 0.143550 0.128549 0.000000 2024-11-06T04:41:07Z- publishEstimatesDecoder() - at 1730868067.02 sec 2024-11-06T04:41:07Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:41:07Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061618.88 594229.76 58.34 2024-11-06T04:41:07Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061618.74 594229.76 57.98 lat/lon: 36.70 -121.95 2024-11-06T04:41:07Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.13 0.69 0.00 2024-11-06T04:41:07Z- AHRS_M2 msg: 1730868067.40 epoch sec; seqNo:805 2024-11-06T04:41:07Z- AHRS_M2 msg: 0.39 phi; -0.07 theta; 0.33 psi 2024-11-06T04:41:07Z- handleDepth()- Depth_Keller msg: 1730868067.40 epoch sec; seqNo:867; depth 52.09 2024-11-06T04:41:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868067.44 epoch sec; seqNo:867; 4061675.13 north; 594225.70 east; 36.70 lat; -121.95 lon; 52.09 depth; utmZone: 10 2024-11-06T04:41:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868067.02 2024-11-06T04:41:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:07Z- waiting for 1730868071.02, time is 1730868067.44 2024-11-06T04:41:07Z- AHRS_M2 msg: 1730868067.80 epoch sec; seqNo:806 2024-11-06T04:41:07Z- AHRS_M2 msg: 0.38 phi; -0.07 theta; 0.36 psi 2024-11-06T04:41:07Z- handleDepth()- Depth_Keller msg: 1730868067.81 epoch sec; seqNo:868; depth 52.12 2024-11-06T04:41:07Z- handleDVL() - DVL msg epochMillisec: 1730868067.81 2024-11-06T04:41:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:41:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:41:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:41:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:41:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:41:07Z- handleDvl() - RDI_Pathfinder msg: 1730868067.81 sec; seqNo:300; #beams:0 2024-11-06T04:41:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:41:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.00 , -0.01 2024-11-06T04:41:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868067.82 epoch sec; seqNo:868; 4061675.51 north; 594225.69 east; 36.70 lat; -121.95 lon; 52.12 depth; utmZone: 10 2024-11-06T04:41:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868067.02 2024-11-06T04:41:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:07Z- waiting for 1730868071.02, time is 1730868067.82 2024-11-06T04:41:08Z- AHRS_M2 msg: 1730868068.21 epoch sec; seqNo:807 2024-11-06T04:41:08Z- AHRS_M2 msg: 0.38 phi; -0.06 theta; 0.40 psi 2024-11-06T04:41:08Z- handleDepth()- Depth_Keller msg: 1730868068.22 epoch sec; seqNo:869; depth 52.12 2024-11-06T04:41:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868068.22 epoch sec; seqNo:869; 4061675.89 north; 594225.69 east; 36.70 lat; -121.95 lon; 52.12 depth; utmZone: 10 2024-11-06T04:41:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868067.02 2024-11-06T04:41:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:08Z- waiting for 1730868071.02, time is 1730868068.22 2024-11-06T04:41:08Z- AHRS_M2 msg: 1730868068.61 epoch sec; seqNo:808 2024-11-06T04:41:08Z- AHRS_M2 msg: 0.37 phi; -0.06 theta; 0.43 psi 2024-11-06T04:41:08Z- handleDepth()- Depth_Keller msg: 1730868068.61 epoch sec; seqNo:870; depth 52.13 2024-11-06T04:41:08Z- handleDVL() - DVL msg epochMillisec: 1730868068.64 2024-11-06T04:41:08Z- handleDvl() - RDI_Pathfinder msg: 1730868068.64 sec; seqNo:301; #beams:4 2024-11-06T04:41:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.10 , 36.46 , 44.63 , 48.39 2024-11-06T04:41:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.01 , -0.05 2024-11-06T04:41:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868068.65 epoch sec; seqNo:870; 4061675.89 north; 594225.69 east; 36.70 lat; -121.95 lon; 52.13 depth; utmZone: 10 2024-11-06T04:41:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868067.02 2024-11-06T04:41:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:08Z- waiting for 1730868071.02, time is 1730868068.65 2024-11-06T04:41:09Z- AHRS_M2 msg: 1730868069.01 epoch sec; seqNo:809 2024-11-06T04:41:09Z- AHRS_M2 msg: 0.37 phi; -0.06 theta; 0.46 psi 2024-11-06T04:41:09Z- handleDepth()- Depth_Keller msg: 1730868069.02 epoch sec; seqNo:871; depth 52.15 2024-11-06T04:41:09Z- handleDVL() - DVL msg epochMillisec: 1730868069.02 2024-11-06T04:41:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:41:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:41:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:41:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:41:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:41:09Z- handleDvl() - RDI_Pathfinder msg: 1730868069.02 sec; seqNo:302; #beams:0 2024-11-06T04:41:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:41:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.01 , -0.05 2024-11-06T04:41:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868069.05 epoch sec; seqNo:871; 4061676.28 north; 594226.91 east; 36.70 lat; -121.95 lon; 52.15 depth; utmZone: 10 2024-11-06T04:41:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868067.02 2024-11-06T04:41:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:09Z- waiting for 1730868071.02, time is 1730868069.05 2024-11-06T04:41:09Z- AHRS_M2 msg: 1730868069.43 epoch sec; seqNo:810 2024-11-06T04:41:09Z- AHRS_M2 msg: 0.37 phi; -0.06 theta; 0.49 psi 2024-11-06T04:41:09Z- handleDepth()- Depth_Keller msg: 1730868069.43 epoch sec; seqNo:872; depth 52.16 2024-11-06T04:41:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868069.44 epoch sec; seqNo:872; 4061676.66 north; 594226.90 east; 36.70 lat; -121.95 lon; 52.16 depth; utmZone: 10 2024-11-06T04:41:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868067.02 2024-11-06T04:41:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:09Z- waiting for 1730868071.02, time is 1730868069.44 2024-11-06T04:41:09Z- handleDepth()- Depth_Keller msg: 1730868069.85 epoch sec; seqNo:873; depth 52.19 2024-11-06T04:41:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868069.86 epoch sec; seqNo:873; 4061677.04 north; 594226.90 east; 36.70 lat; -121.95 lon; 52.19 depth; utmZone: 10 2024-11-06T04:41:10Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:41:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868067.02 2024-11-06T04:41:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:10Z- waiting for 1730868071.02, time is 1730868069.86 2024-11-06T04:41:10Z- handleDepth()- Depth_Keller msg: 1730868070.25 epoch sec; seqNo:874; depth 52.19 2024-11-06T04:41:10Z- handleDVL() - DVL msg epochMillisec: 1730868070.25 2024-11-06T04:41:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:41:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:41:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:41:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:41:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:41:10Z- handleDvl() - RDI_Pathfinder msg: 1730868070.25 sec; seqNo:303; #beams:0 2024-11-06T04:41:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:41:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.01 , -0.05 2024-11-06T04:41:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868070.26 epoch sec; seqNo:874; 4061677.04 north; 594226.90 east; 36.70 lat; -121.95 lon; 52.19 depth; utmZone: 10 2024-11-06T04:41:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868067.02 2024-11-06T04:41:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:10Z- waiting for 1730868071.02, time is 1730868070.26 2024-11-06T04:41:10Z- AHRS_M2 msg: 1730868070.63 epoch sec; seqNo:811 2024-11-06T04:41:10Z- AHRS_M2 msg: 0.39 phi; -0.05 theta; 0.59 psi 2024-11-06T04:41:10Z- handleDepth()- Depth_Keller msg: 1730868070.64 epoch sec; seqNo:875; depth 52.21 2024-11-06T04:41:10Z- handleDVL() - DVL msg epochMillisec: 1730868070.66 2024-11-06T04:41:10Z- handleDvl() - RDI_Pathfinder msg: 1730868070.66 sec; seqNo:304; #beams:4 2024-11-06T04:41:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 59.88 , 37.78 , 44.41 , 47.06 2024-11-06T04:41:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.05 , -0.07 2024-11-06T04:41:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868070.69 epoch sec; seqNo:875; 4061677.42 north; 594226.89 east; 36.70 lat; -121.95 lon; 52.21 depth; utmZone: 10 2024-11-06T04:41:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868067.02 2024-11-06T04:41:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:10Z- waiting for 1730868071.02, time is 1730868070.69 2024-11-06T04:41:11Z- AHRS_M2 msg: 1730868071.03 epoch sec; seqNo:812 2024-11-06T04:41:11Z- AHRS_M2 msg: 0.40 phi; -0.05 theta; 0.62 psi 2024-11-06T04:41:11Z- handleDepth()- Depth_Keller msg: 1730868071.04 epoch sec; seqNo:876; depth 52.22 2024-11-06T04:41:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868071.05 epoch sec; seqNo:876; 4061677.79 north; 594226.89 east; 36.70 lat; -121.95 lon; 52.22 depth; utmZone: 10 2024-11-06T04:41:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868067.02 2024-11-06T04:41:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:11Z- waiting for 1730868071.02, time is 1730868071.05 2024-11-06T04:41:11Z- LcmTrn::updateTrn() >>>> pose time 1730868071.05, meas time 1730868070.66 <<<< 2024-11-06T04:41:11Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868071.05 2024-11-06T04:41:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868070.66 sec, ping # 0; waiting for INS data more recent than 1730868067.02... 2024-11-06T04:41:11Z- TerrainNav::motionUpdate() - currEstimate = (4061678, 594226.89, 52.22) 2024-11-06T04:41:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:41:11Z- TerrainNav:: SubcloudNIS is 0.15, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:41:11Z- TNavPF::Projecting Measurements 2024-11-06T04:41:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:41:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:41:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:41:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:41:11Z- TNavFilter::4 good beams. 2024-11-06T04:41:11Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:41:11Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:41:11Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:41:11Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:41:11Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:41:11Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:41:11Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:41:11Z- TerrainMapOctree: (vehN, vehE ) = (4061677.50, 594226.77). 2024-11-06T04:41:11Z- TerrainMapOctree: Min Distance = 76.13. 2024-11-06T04:41:11Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:41:11Z- Forcing Subcloud Comparison 2024-11-06T04:41:11Z- Weighting particles with subcloud comparison 2024-11-06T04:41:11Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:11Z- meanExpectedMeasDiff: -2.247071 delta_rms_squared: 2.567237 alpha: 0.992270 2024-11-06T04:41:11Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:11Z- meanExpectedMeasDiff: 0.894297 delta_rms_squared: 0.949035 alpha: 0.979361 2024-11-06T04:41:11Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:11Z- meanExpectedMeasDiff: 0.382795 delta_rms_squared: 0.907437 alpha: 0.978435 2024-11-06T04:41:11Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:11Z- meanExpectedMeasDiff: -1.505872 delta_rms_squared: 1.565175 alpha: 0.987383 2024-11-06T04:41:11Z- Subcloud NIS: 0.157899 2024-11-06T04:41:11Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:41:11Z- TNavPF:: sumWeights = 0.388918 2024-11-06T04:41:11Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:41:11Z- TNavPF::First Term of Map Covariance Matrix is 0.96 2024-11-06T04:41:11Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T04:41:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868070.66 sec. 2024-11-06T04:41:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:41:11Z- variances([0], [2], and psi_berg) = 0.143175 0.131316 0.000000 2024-11-06T04:41:11Z- publishEstimatesDecoder() - at 1730868071.05 sec 2024-11-06T04:41:11Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:41:11Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061621.45 594231.07 58.70 2024-11-06T04:41:11Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061621.77 594230.95 58.42 lat/lon: 36.70 -121.95 2024-11-06T04:41:11Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.13 0.71 0.00 2024-11-06T04:41:11Z- AHRS_M2 msg: 1730868071.47 epoch sec; seqNo:813 2024-11-06T04:41:11Z- AHRS_M2 msg: 0.39 phi; -0.04 theta; 0.65 psi 2024-11-06T04:41:11Z- handleDepth()- Depth_Keller msg: 1730868071.48 epoch sec; seqNo:877; depth 52.24 2024-11-06T04:41:11Z- handleDVL() - DVL msg epochMillisec: 1730868071.48 2024-11-06T04:41:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:41:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:41:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:41:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:41:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:41:11Z- handleDvl() - RDI_Pathfinder msg: 1730868071.48 sec; seqNo:305; #beams:0 2024-11-06T04:41:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:41:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.05 , -0.07 2024-11-06T04:41:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868071.51 epoch sec; seqNo:877; 4061677.79 north; 594226.89 east; 36.70 lat; -121.95 lon; 52.24 depth; utmZone: 10 2024-11-06T04:41:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868071.05 2024-11-06T04:41:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:11Z- waiting for 1730868075.05, time is 1730868071.51 2024-11-06T04:41:11Z- handleDepth()- Depth_Keller msg: 1730868071.87 epoch sec; seqNo:878; depth 52.27 2024-11-06T04:41:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868071.88 epoch sec; seqNo:878; 4061678.19 north; 594228.10 east; 36.70 lat; -121.95 lon; 52.27 depth; utmZone: 10 2024-11-06T04:41:12Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:41:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868071.05 2024-11-06T04:41:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:12Z- waiting for 1730868075.05, time is 1730868071.88 2024-11-06T04:41:12Z- AHRS_M2 msg: 1730868072.24 epoch sec; seqNo:814 2024-11-06T04:41:12Z- AHRS_M2 msg: 0.39 phi; -0.03 theta; 0.72 psi 2024-11-06T04:41:12Z- handleDepth()- Depth_Keller msg: 1730868072.25 epoch sec; seqNo:879; depth 52.26 2024-11-06T04:41:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868072.27 epoch sec; seqNo:879; 4061678.57 north; 594228.10 east; 36.70 lat; -121.95 lon; 52.26 depth; utmZone: 10 2024-11-06T04:41:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868071.05 2024-11-06T04:41:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:12Z- waiting for 1730868075.05, time is 1730868072.27 2024-11-06T04:41:12Z- handleDepth()- Depth_Keller msg: 1730868072.67 epoch sec; seqNo:880; depth 52.29 2024-11-06T04:41:12Z- handleDVL() - DVL msg epochMillisec: 1730868072.69 2024-11-06T04:41:12Z- handleDvl() - RDI_Pathfinder msg: 1730868072.69 sec; seqNo:306; #beams:4 2024-11-06T04:41:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.98 , 36.46 , 44.85 , 47.06 2024-11-06T04:41:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.09 , -0.05 2024-11-06T04:41:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868072.71 epoch sec; seqNo:880; 4061678.57 north; 594228.10 east; 36.70 lat; -121.95 lon; 52.29 depth; utmZone: 10 2024-11-06T04:41:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868071.05 2024-11-06T04:41:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:12Z- waiting for 1730868075.05, time is 1730868072.71 2024-11-06T04:41:13Z- AHRS_M2 msg: 1730868073.05 epoch sec; seqNo:815 2024-11-06T04:41:13Z- AHRS_M2 msg: 0.39 phi; -0.03 theta; 0.78 psi 2024-11-06T04:41:13Z- handleDepth()- Depth_Keller msg: 1730868073.06 epoch sec; seqNo:881; depth 52.29 2024-11-06T04:41:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868073.08 epoch sec; seqNo:881; 4061678.94 north; 594228.10 east; 36.70 lat; -121.95 lon; 52.29 depth; utmZone: 10 2024-11-06T04:41:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868071.05 2024-11-06T04:41:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:13Z- waiting for 1730868075.05, time is 1730868073.08 2024-11-06T04:41:13Z- AHRS_M2 msg: 1730868073.45 epoch sec; seqNo:816 2024-11-06T04:41:13Z- AHRS_M2 msg: 0.39 phi; -0.02 theta; 0.81 psi 2024-11-06T04:41:13Z- handleDepth()- Depth_Keller msg: 1730868073.46 epoch sec; seqNo:882; depth 52.31 2024-11-06T04:41:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868073.47 epoch sec; seqNo:882; 4061678.94 north; 594228.10 east; 36.70 lat; -121.95 lon; 52.31 depth; utmZone: 10 2024-11-06T04:41:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868071.05 2024-11-06T04:41:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:13Z- waiting for 1730868075.05, time is 1730868073.47 2024-11-06T04:41:13Z- AHRS_M2 msg: 1730868073.86 epoch sec; seqNo:817 2024-11-06T04:41:13Z- AHRS_M2 msg: 0.39 phi; -0.02 theta; 0.84 psi 2024-11-06T04:41:13Z- handleDepth()- Depth_Keller msg: 1730868073.87 epoch sec; seqNo:883; depth 52.32 2024-11-06T04:41:13Z- handleDVL() - DVL msg epochMillisec: 1730868073.87 2024-11-06T04:41:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:41:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:41:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:41:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:41:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:41:13Z- handleDvl() - RDI_Pathfinder msg: 1730868073.87 sec; seqNo:307; #beams:0 2024-11-06T04:41:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:41:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.09 , -0.05 2024-11-06T04:41:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868073.88 epoch sec; seqNo:883; 4061679.32 north; 594228.09 east; 36.70 lat; -121.95 lon; 52.32 depth; utmZone: 10 2024-11-06T04:41:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868071.05 2024-11-06T04:41:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:14Z- waiting for 1730868075.05, time is 1730868073.88 2024-11-06T04:41:14Z- AHRS_M2 msg: 1730868074.26 epoch sec; seqNo:818 2024-11-06T04:41:14Z- AHRS_M2 msg: 0.38 phi; -0.02 theta; 0.88 psi 2024-11-06T04:41:14Z- handleDepth()- Depth_Keller msg: 1730868074.28 epoch sec; seqNo:884; depth 52.33 2024-11-06T04:41:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868074.29 epoch sec; seqNo:884; 4061679.70 north; 594228.09 east; 36.70 lat; -121.95 lon; 52.33 depth; utmZone: 10 2024-11-06T04:41:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868071.05 2024-11-06T04:41:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:14Z- waiting for 1730868075.05, time is 1730868074.29 2024-11-06T04:41:14Z- AHRS_M2 msg: 1730868074.67 epoch sec; seqNo:819 2024-11-06T04:41:14Z- AHRS_M2 msg: 0.38 phi; -0.01 theta; 0.91 psi 2024-11-06T04:41:14Z- handleDepth()- Depth_Keller msg: 1730868074.67 epoch sec; seqNo:885; depth 52.33 2024-11-06T04:41:14Z- handleDVL() - DVL msg epochMillisec: 1730868074.70 2024-11-06T04:41:14Z- handleDvl() - RDI_Pathfinder msg: 1730868074.70 sec; seqNo:308; #beams:4 2024-11-06T04:41:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.10 , 37.12 , 45.51 , 46.40 2024-11-06T04:41:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.08 , -0.01 2024-11-06T04:41:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868074.71 epoch sec; seqNo:885; 4061679.72 north; 594229.31 east; 36.70 lat; -121.95 lon; 52.33 depth; utmZone: 10 2024-11-06T04:41:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868071.05 2024-11-06T04:41:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:14Z- waiting for 1730868075.05, time is 1730868074.71 2024-11-06T04:41:15Z- AHRS_M2 msg: 1730868075.07 epoch sec; seqNo:820 2024-11-06T04:41:15Z- AHRS_M2 msg: 0.38 phi; -0.01 theta; 0.94 psi 2024-11-06T04:41:15Z- handleDepth()- Depth_Keller msg: 1730868075.08 epoch sec; seqNo:886; depth 52.36 2024-11-06T04:41:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868075.09 epoch sec; seqNo:886; 4061680.09 north; 594229.30 east; 36.70 lat; -121.95 lon; 52.36 depth; utmZone: 10 2024-11-06T04:41:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868071.05 2024-11-06T04:41:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:15Z- waiting for 1730868075.05, time is 1730868075.09 2024-11-06T04:41:15Z- LcmTrn::updateTrn() >>>> pose time 1730868075.09, meas time 1730868074.70 <<<< 2024-11-06T04:41:15Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868075.09 2024-11-06T04:41:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868074.70 sec, ping # 0; waiting for INS data more recent than 1730868071.05... 2024-11-06T04:41:15Z- TerrainNav::motionUpdate() - currEstimate = (4061680, 594229.30, 52.36) 2024-11-06T04:41:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:41:15Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:41:15Z- TNavPF::Projecting Measurements 2024-11-06T04:41:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:41:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:41:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:41:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:41:15Z- TNavFilter::4 good beams. 2024-11-06T04:41:15Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:41:15Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:41:15Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:41:15Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:41:15Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:41:15Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:41:15Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:41:15Z- TerrainMapOctree: (vehN, vehE ) = (4061679.87, 594229.07). 2024-11-06T04:41:15Z- TerrainMapOctree: Min Distance = 73.18. 2024-11-06T04:41:15Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:41:15Z- Forcing Subcloud Comparison 2024-11-06T04:41:15Z- Weighting particles with subcloud comparison 2024-11-06T04:41:15Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:15Z- meanExpectedMeasDiff: -0.003225 delta_rms_squared: 2.506929 alpha: 0.992085 2024-11-06T04:41:15Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:15Z- meanExpectedMeasDiff: 0.609907 delta_rms_squared: 0.970914 alpha: 0.979817 2024-11-06T04:41:15Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:15Z- meanExpectedMeasDiff: 2.124326 delta_rms_squared: 1.455537 alpha: 0.986446 2024-11-06T04:41:15Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:15Z- meanExpectedMeasDiff: -0.142857 delta_rms_squared: 1.499342 alpha: 0.986836 2024-11-06T04:41:15Z- Subcloud NIS: 0.162567 2024-11-06T04:41:15Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:41:15Z- TNavPF:: sumWeights = 0.453603 2024-11-06T04:41:15Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:41:15Z- TNavPF::First Term of Map Covariance Matrix is 0.57 2024-11-06T04:41:15Z- TNavPF::Calculated NIS Value : 0.19 numBeams normalized NIS: 0.05 2024-11-06T04:41:15Z- TerrainNav::Resampling particle filter... 2024-11-06T04:41:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868074.70 sec. 2024-11-06T04:41:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:41:15Z- variances([0], [2], and psi_berg) = 0.145835 0.132211 0.000000 2024-11-06T04:41:15Z- publishEstimatesDecoder() - at 1730868075.09 sec 2024-11-06T04:41:15Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:41:15Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061624.17 594233.28 56.81 2024-11-06T04:41:15Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061624.07 594233.36 58.29 lat/lon: 36.70 -121.95 2024-11-06T04:41:15Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.13 0.71 0.00 2024-11-06T04:41:15Z- AHRS_M2 msg: 1730868075.49 epoch sec; seqNo:821 2024-11-06T04:41:15Z- AHRS_M2 msg: 0.38 phi; -0.00 theta; 0.97 psi 2024-11-06T04:41:15Z- handleDepth()- Depth_Keller msg: 1730868075.51 epoch sec; seqNo:887; depth 52.36 2024-11-06T04:41:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868075.53 epoch sec; seqNo:887; 4061680.09 north; 594229.30 east; 36.70 lat; -121.95 lon; 52.36 depth; utmZone: 10 2024-11-06T04:41:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868075.09 2024-11-06T04:41:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:15Z- waiting for 1730868079.09, time is 1730868075.53 2024-11-06T04:41:15Z- AHRS_M2 msg: 1730868075.88 epoch sec; seqNo:822 2024-11-06T04:41:15Z- AHRS_M2 msg: 0.38 phi; -0.00 theta; 1.00 psi 2024-11-06T04:41:15Z- handleDepth()- Depth_Keller msg: 1730868075.88 epoch sec; seqNo:888; depth 52.35 2024-11-06T04:41:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868075.89 epoch sec; seqNo:888; 4061680.47 north; 594229.30 east; 36.70 lat; -121.95 lon; 52.35 depth; utmZone: 10 2024-11-06T04:41:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868075.09 2024-11-06T04:41:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:16Z- waiting for 1730868079.09, time is 1730868075.89 2024-11-06T04:41:16Z- AHRS_M2 msg: 1730868076.28 epoch sec; seqNo:823 2024-11-06T04:41:16Z- AHRS_M2 msg: 0.38 phi; 0.00 theta; 1.04 psi 2024-11-06T04:41:16Z- handleDepth()- Depth_Keller msg: 1730868076.29 epoch sec; seqNo:889; depth 52.37 2024-11-06T04:41:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868076.31 epoch sec; seqNo:889; 4061680.47 north; 594229.30 east; 36.70 lat; -121.95 lon; 52.37 depth; utmZone: 10 2024-11-06T04:41:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868075.09 2024-11-06T04:41:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:16Z- waiting for 1730868079.09, time is 1730868076.31 2024-11-06T04:41:16Z- AHRS_M2 msg: 1730868076.69 epoch sec; seqNo:824 2024-11-06T04:41:16Z- AHRS_M2 msg: 0.38 phi; 0.00 theta; 1.07 psi 2024-11-06T04:41:16Z- handleDepth()- Depth_Keller msg: 1730868076.70 epoch sec; seqNo:890; depth 52.36 2024-11-06T04:41:16Z- handleDVL() - DVL msg epochMillisec: 1730868076.72 2024-11-06T04:41:16Z- handleDvl() - RDI_Pathfinder msg: 1730868076.72 sec; seqNo:309; #beams:4 2024-11-06T04:41:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 59.65 , 36.01 , 45.51 , 45.95 2024-11-06T04:41:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.03 , 0.04 2024-11-06T04:41:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868076.74 epoch sec; seqNo:890; 4061680.87 north; 594230.52 east; 36.70 lat; -121.95 lon; 52.36 depth; utmZone: 10 2024-11-06T04:41:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868075.09 2024-11-06T04:41:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:16Z- waiting for 1730868079.09, time is 1730868076.74 2024-11-06T04:41:17Z- handleDepth()- Depth_Keller msg: 1730868077.11 epoch sec; seqNo:891; depth 52.38 2024-11-06T04:41:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868077.13 epoch sec; seqNo:891; 4061680.87 north; 594230.52 east; 36.70 lat; -121.95 lon; 52.38 depth; utmZone: 10 2024-11-06T04:41:17Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:41:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868075.09 2024-11-06T04:41:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:17Z- waiting for 1730868079.09, time is 1730868077.13 2024-11-06T04:41:17Z- AHRS_M2 msg: 1730868077.51 epoch sec; seqNo:825 2024-11-06T04:41:17Z- AHRS_M2 msg: 0.39 phi; 0.01 theta; 1.17 psi 2024-11-06T04:41:17Z- handleDepth()- Depth_Keller msg: 1730868077.51 epoch sec; seqNo:892; depth 52.36 2024-11-06T04:41:17Z- handleDVL() - DVL msg epochMillisec: 1730868077.51 2024-11-06T04:41:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:41:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:41:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:41:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:41:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:41:17Z- handleDvl() - RDI_Pathfinder msg: 1730868077.51 sec; seqNo:310; #beams:0 2024-11-06T04:41:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:41:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.03 , 0.04 2024-11-06T04:41:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868077.52 epoch sec; seqNo:892; 4061680.87 north; 594230.52 east; 36.70 lat; -121.95 lon; 52.36 depth; utmZone: 10 2024-11-06T04:41:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868075.09 2024-11-06T04:41:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:17Z- waiting for 1730868079.09, time is 1730868077.52 2024-11-06T04:41:17Z- AHRS_M2 msg: 1730868077.90 epoch sec; seqNo:826 2024-11-06T04:41:17Z- AHRS_M2 msg: 0.38 phi; 0.01 theta; 1.20 psi 2024-11-06T04:41:17Z- handleDepth()- Depth_Keller msg: 1730868077.91 epoch sec; seqNo:893; depth 52.37 2024-11-06T04:41:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868077.92 epoch sec; seqNo:893; 4061681.24 north; 594230.51 east; 36.70 lat; -121.95 lon; 52.37 depth; utmZone: 10 2024-11-06T04:41:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868075.09 2024-11-06T04:41:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:18Z- waiting for 1730868079.09, time is 1730868077.92 2024-11-06T04:41:18Z- AHRS_M2 msg: 1730868078.30 epoch sec; seqNo:827 2024-11-06T04:41:18Z- AHRS_M2 msg: 0.38 phi; 0.02 theta; 1.24 psi 2024-11-06T04:41:18Z- handleDepth()- Depth_Keller msg: 1730868078.31 epoch sec; seqNo:894; depth 52.37 2024-11-06T04:41:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868078.32 epoch sec; seqNo:894; 4061681.26 north; 594231.73 east; 36.70 lat; -121.95 lon; 52.37 depth; utmZone: 10 2024-11-06T04:41:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868075.09 2024-11-06T04:41:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:18Z- waiting for 1730868079.09, time is 1730868078.32 2024-11-06T04:41:18Z- AHRS_M2 msg: 1730868078.71 epoch sec; seqNo:828 2024-11-06T04:41:18Z- AHRS_M2 msg: 0.38 phi; 0.02 theta; 1.27 psi 2024-11-06T04:41:18Z- handleDepth()- Depth_Keller msg: 1730868078.72 epoch sec; seqNo:895; depth 52.37 2024-11-06T04:41:18Z- handleDVL() - DVL msg epochMillisec: 1730868078.74 2024-11-06T04:41:18Z- handleDvl() - RDI_Pathfinder msg: 1730868078.74 sec; seqNo:311; #beams:4 2024-11-06T04:41:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.54 , 37.78 , 45.51 , 45.96 2024-11-06T04:41:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.01 , 0.02 2024-11-06T04:41:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868078.77 epoch sec; seqNo:895; 4061681.26 north; 594231.73 east; 36.70 lat; -121.95 lon; 52.37 depth; utmZone: 10 2024-11-06T04:41:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868075.09 2024-11-06T04:41:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:18Z- waiting for 1730868079.09, time is 1730868078.77 2024-11-06T04:41:19Z- AHRS_M2 msg: 1730868079.11 epoch sec; seqNo:829 2024-11-06T04:41:19Z- AHRS_M2 msg: 0.37 phi; 0.03 theta; 1.31 psi 2024-11-06T04:41:19Z- handleDepth()- Depth_Keller msg: 1730868079.12 epoch sec; seqNo:896; depth 52.37 2024-11-06T04:41:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868079.13 epoch sec; seqNo:896; 4061681.26 north; 594231.73 east; 36.70 lat; -121.95 lon; 52.37 depth; utmZone: 10 2024-11-06T04:41:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868075.09 2024-11-06T04:41:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:19Z- waiting for 1730868079.09, time is 1730868079.13 2024-11-06T04:41:19Z- LcmTrn::updateTrn() >>>> pose time 1730868079.13, meas time 1730868078.74 <<<< 2024-11-06T04:41:19Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868079.13 2024-11-06T04:41:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868078.74 sec, ping # 0; waiting for INS data more recent than 1730868075.09... 2024-11-06T04:41:19Z- TerrainNav::motionUpdate() - currEstimate = (4061681, 594231.73, 52.37) 2024-11-06T04:41:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:41:19Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:41:19Z- TNavPF::Projecting Measurements 2024-11-06T04:41:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:41:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:41:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:41:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:41:19Z- TNavFilter::4 good beams. 2024-11-06T04:41:19Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:41:19Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:41:19Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:41:19Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:41:19Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:41:19Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:41:19Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:41:19Z- TerrainMapOctree: (vehN, vehE ) = (4061681.14, 594231.49). 2024-11-06T04:41:19Z- TerrainMapOctree: Min Distance = 71.30. 2024-11-06T04:41:19Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:41:19Z- Forcing Subcloud Comparison 2024-11-06T04:41:19Z- Weighting particles with subcloud comparison 2024-11-06T04:41:19Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:19Z- meanExpectedMeasDiff: 2.861670 delta_rms_squared: 1.796645 alpha: 0.988991 2024-11-06T04:41:19Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:19Z- meanExpectedMeasDiff: 0.988631 delta_rms_squared: 0.970394 alpha: 0.979806 2024-11-06T04:41:19Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:19Z- meanExpectedMeasDiff: 0.963926 delta_rms_squared: 1.509893 alpha: 0.986927 2024-11-06T04:41:19Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:19Z- meanExpectedMeasDiff: 0.215761 delta_rms_squared: 1.425249 alpha: 0.986162 2024-11-06T04:41:19Z- Subcloud NIS: 0.170144 2024-11-06T04:41:19Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:41:19Z- TNavPF:: sumWeights = 0.424491 2024-11-06T04:41:19Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:41:19Z- TNavPF::First Term of Map Covariance Matrix is 1.03 2024-11-06T04:41:19Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T04:41:19Z- TerrainNav::Resampling particle filter... 2024-11-06T04:41:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868078.74 sec. 2024-11-06T04:41:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:41:19Z- variances([0], [2], and psi_berg) = 0.144028 0.132982 0.000000 2024-11-06T04:41:19Z- publishEstimatesDecoder() - at 1730868079.13 sec 2024-11-06T04:41:19Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:41:19Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061625.36 594235.76 56.34 2024-11-06T04:41:19Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061625.21 594235.76 57.86 lat/lon: 36.70 -121.95 2024-11-06T04:41:19Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.13 0.74 0.00 2024-11-06T04:41:19Z- AHRS_M2 msg: 1730868079.52 epoch sec; seqNo:830 2024-11-06T04:41:19Z- AHRS_M2 msg: 0.37 phi; 0.03 theta; 1.34 psi 2024-11-06T04:41:19Z- handleDepth()- Depth_Keller msg: 1730868079.54 epoch sec; seqNo:897; depth 52.36 2024-11-06T04:41:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868079.57 epoch sec; seqNo:897; 4061681.64 north; 594231.73 east; 36.70 lat; -121.95 lon; 52.36 depth; utmZone: 10 2024-11-06T04:41:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868079.13 2024-11-06T04:41:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:19Z- waiting for 1730868083.13, time is 1730868079.57 2024-11-06T04:41:19Z- AHRS_M2 msg: 1730868079.92 epoch sec; seqNo:831 2024-11-06T04:41:19Z- AHRS_M2 msg: 0.37 phi; 0.04 theta; 1.38 psi 2024-11-06T04:41:19Z- handleDepth()- Depth_Keller msg: 1730868079.92 epoch sec; seqNo:898; depth 52.35 2024-11-06T04:41:19Z- handleDVL() - DVL msg epochMillisec: 1730868079.93 2024-11-06T04:41:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:41:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:41:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:41:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:41:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:41:19Z- handleDvl() - RDI_Pathfinder msg: 1730868079.93 sec; seqNo:312; #beams:0 2024-11-06T04:41:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:41:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.01 , 0.02 2024-11-06T04:41:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868079.93 epoch sec; seqNo:898; 4061681.65 north; 594232.95 east; 36.70 lat; -121.95 lon; 52.35 depth; utmZone: 10 2024-11-06T04:41:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868079.13 2024-11-06T04:41:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:20Z- waiting for 1730868083.13, time is 1730868079.93 2024-11-06T04:41:20Z- AHRS_M2 msg: 1730868080.39 epoch sec; seqNo:832 2024-11-06T04:41:20Z- AHRS_M2 msg: 0.38 phi; 0.04 theta; 1.42 psi 2024-11-06T04:41:20Z- handleDepth()- Depth_Keller msg: 1730868080.41 epoch sec; seqNo:899; depth 52.34 2024-11-06T04:41:20Z- handleDVL() - DVL msg epochMillisec: 1730868080.44 2024-11-06T04:41:20Z- handleDvl() - RDI_Pathfinder msg: 1730868080.44 sec; seqNo:313; #beams:4 2024-11-06T04:41:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 59.43 , 37.56 , 45.29 , 44.85 2024-11-06T04:41:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.01 , -0.01 2024-11-06T04:41:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868080.48 epoch sec; seqNo:899; 4061681.65 north; 594232.95 east; 36.70 lat; -121.95 lon; 52.34 depth; utmZone: 10 2024-11-06T04:41:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868079.13 2024-11-06T04:41:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:20Z- waiting for 1730868083.13, time is 1730868080.48 2024-11-06T04:41:20Z- AHRS_M2 msg: 1730868080.72 epoch sec; seqNo:833 2024-11-06T04:41:20Z- AHRS_M2 msg: 0.40 phi; 0.04 theta; 1.46 psi 2024-11-06T04:41:20Z- handleDepth()- Depth_Keller msg: 1730868080.73 epoch sec; seqNo:900; depth 52.33 2024-11-06T04:41:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868080.75 epoch sec; seqNo:900; 4061681.65 north; 594232.95 east; 36.70 lat; -121.95 lon; 52.33 depth; utmZone: 10 2024-11-06T04:41:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868079.13 2024-11-06T04:41:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:20Z- waiting for 1730868083.13, time is 1730868080.75 2024-11-06T04:41:21Z- AHRS_M2 msg: 1730868081.13 epoch sec; seqNo:834 2024-11-06T04:41:21Z- AHRS_M2 msg: 0.41 phi; 0.04 theta; 1.49 psi 2024-11-06T04:41:21Z- handleDepth()- Depth_Keller msg: 1730868081.13 epoch sec; seqNo:901; depth 52.31 2024-11-06T04:41:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868081.14 epoch sec; seqNo:901; 4061681.65 north; 594232.95 east; 36.70 lat; -121.95 lon; 52.31 depth; utmZone: 10 2024-11-06T04:41:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868079.13 2024-11-06T04:41:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:21Z- waiting for 1730868083.13, time is 1730868081.14 2024-11-06T04:41:21Z- AHRS_M2 msg: 1730868081.54 epoch sec; seqNo:835 2024-11-06T04:41:21Z- AHRS_M2 msg: 0.42 phi; 0.04 theta; 1.53 psi 2024-11-06T04:41:21Z- handleDepth()- Depth_Keller msg: 1730868081.54 epoch sec; seqNo:902; depth 52.29 2024-11-06T04:41:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868081.55 epoch sec; seqNo:902; 4061681.66 north; 594234.17 east; 36.70 lat; -121.95 lon; 52.29 depth; utmZone: 10 2024-11-06T04:41:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868079.13 2024-11-06T04:41:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:21Z- waiting for 1730868083.13, time is 1730868081.55 2024-11-06T04:41:21Z- AHRS_M2 msg: 1730868081.93 epoch sec; seqNo:836 2024-11-06T04:41:21Z- AHRS_M2 msg: 0.41 phi; 0.04 theta; 1.56 psi 2024-11-06T04:41:21Z- handleDepth()- Depth_Keller msg: 1730868081.94 epoch sec; seqNo:903; depth 52.27 2024-11-06T04:41:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868081.95 epoch sec; seqNo:903; 4061681.66 north; 594234.17 east; 36.70 lat; -121.95 lon; 52.27 depth; utmZone: 10 2024-11-06T04:41:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868079.13 2024-11-06T04:41:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:22Z- waiting for 1730868083.13, time is 1730868081.95 2024-11-06T04:41:22Z- AHRS_M2 msg: 1730868082.35 epoch sec; seqNo:837 2024-11-06T04:41:22Z- AHRS_M2 msg: 0.41 phi; 0.04 theta; 1.60 psi 2024-11-06T04:41:22Z- handleDepth()- Depth_Keller msg: 1730868082.38 epoch sec; seqNo:904; depth 52.24 2024-11-06T04:41:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868082.41 epoch sec; seqNo:904; 4061681.66 north; 594234.17 east; 36.70 lat; -121.95 lon; 52.24 depth; utmZone: 10 2024-11-06T04:41:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868079.13 2024-11-06T04:41:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:22Z- waiting for 1730868083.13, time is 1730868082.41 2024-11-06T04:41:22Z- AHRS_M2 msg: 1730868082.74 epoch sec; seqNo:838 2024-11-06T04:41:22Z- AHRS_M2 msg: 0.39 phi; 0.04 theta; 1.63 psi 2024-11-06T04:41:22Z- handleDepth()- Depth_Keller msg: 1730868082.75 epoch sec; seqNo:905; depth 52.22 2024-11-06T04:41:22Z- handleDVL() - DVL msg epochMillisec: 1730868082.78 2024-11-06T04:41:22Z- handleDvl() - RDI_Pathfinder msg: 1730868082.78 sec; seqNo:314; #beams:4 2024-11-06T04:41:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.53 , 37.56 , 46.17 , 45.73 2024-11-06T04:41:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.02 , -0.03 2024-11-06T04:41:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868082.80 epoch sec; seqNo:905; 4061681.66 north; 594234.17 east; 36.70 lat; -121.95 lon; 52.22 depth; utmZone: 10 2024-11-06T04:41:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868079.13 2024-11-06T04:41:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:22Z- waiting for 1730868083.13, time is 1730868082.80 2024-11-06T04:41:23Z- AHRS_M2 msg: 1730868083.15 epoch sec; seqNo:839 2024-11-06T04:41:23Z- AHRS_M2 msg: 0.37 phi; 0.04 theta; 1.66 psi 2024-11-06T04:41:23Z- handleDepth()- Depth_Keller msg: 1730868083.15 epoch sec; seqNo:906; depth 52.20 2024-11-06T04:41:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868083.16 epoch sec; seqNo:906; 4061681.68 north; 594235.39 east; 36.70 lat; -121.95 lon; 52.20 depth; utmZone: 10 2024-11-06T04:41:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868079.13 2024-11-06T04:41:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:23Z- waiting for 1730868083.13, time is 1730868083.16 2024-11-06T04:41:23Z- LcmTrn::updateTrn() >>>> pose time 1730868083.16, meas time 1730868082.78 <<<< 2024-11-06T04:41:23Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868083.16 2024-11-06T04:41:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868082.78 sec, ping # 0; waiting for INS data more recent than 1730868079.13... 2024-11-06T04:41:23Z- TerrainNav::motionUpdate() - currEstimate = (4061682, 594235.39, 52.20) 2024-11-06T04:41:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:41:23Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:41:23Z- TNavPF::Projecting Measurements 2024-11-06T04:41:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:41:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:41:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:41:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:41:23Z- TNavFilter::4 good beams. 2024-11-06T04:41:23Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:41:23Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:41:23Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:41:23Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:41:23Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:41:23Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:41:23Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:41:23Z- TerrainMapOctree: (vehN, vehE ) = (4061681.64, 594235.04). 2024-11-06T04:41:23Z- TerrainMapOctree: Min Distance = 69.98. 2024-11-06T04:41:23Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:41:23Z- Forcing Subcloud Comparison 2024-11-06T04:41:23Z- Weighting particles with subcloud comparison 2024-11-06T04:41:23Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:23Z- meanExpectedMeasDiff: 2.753887 delta_rms_squared: 2.576490 alpha: 0.992297 2024-11-06T04:41:23Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:23Z- meanExpectedMeasDiff: 0.142712 delta_rms_squared: 1.016666 alpha: 0.980707 2024-11-06T04:41:23Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:23Z- meanExpectedMeasDiff: 0.455700 delta_rms_squared: 1.609311 alpha: 0.987725 2024-11-06T04:41:23Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:23Z- meanExpectedMeasDiff: 0.857748 delta_rms_squared: 1.466825 alpha: 0.986549 2024-11-06T04:41:23Z- Subcloud NIS: 0.173173 2024-11-06T04:41:23Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:41:23Z- TNavPF:: sumWeights = 0.457548 2024-11-06T04:41:23Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:41:23Z- TNavPF::First Term of Map Covariance Matrix is 1.11 2024-11-06T04:41:23Z- TNavPF::Calculated NIS Value : 0.14 numBeams normalized NIS: 0.04 2024-11-06T04:41:23Z- TerrainNav::Resampling particle filter... 2024-11-06T04:41:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868082.78 sec. 2024-11-06T04:41:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:41:23Z- variances([0], [2], and psi_berg) = 0.147431 0.136923 0.000000 2024-11-06T04:41:23Z- publishEstimatesDecoder() - at 1730868083.16 sec 2024-11-06T04:41:23Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:41:23Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061625.73 594239.50 56.26 2024-11-06T04:41:23Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061625.62 594239.42 57.26 lat/lon: 36.70 -121.95 2024-11-06T04:41:23Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.14 0.70 0.00 2024-11-06T04:41:23Z- AHRS_M2 msg: 1730868083.57 epoch sec; seqNo:840 2024-11-06T04:41:23Z- AHRS_M2 msg: 0.37 phi; 0.04 theta; 1.69 psi 2024-11-06T04:41:23Z- handleDepth()- Depth_Keller msg: 1730868083.60 epoch sec; seqNo:907; depth 52.19 2024-11-06T04:41:23Z- handleDVL() - DVL msg epochMillisec: 1730868083.60 2024-11-06T04:41:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:41:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:41:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:41:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:41:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:41:23Z- handleDvl() - RDI_Pathfinder msg: 1730868083.60 sec; seqNo:315; #beams:0 2024-11-06T04:41:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:41:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.02 , -0.03 2024-11-06T04:41:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868083.62 epoch sec; seqNo:907; 4061681.68 north; 594235.39 east; 36.70 lat; -121.95 lon; 52.19 depth; utmZone: 10 2024-11-06T04:41:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868083.16 2024-11-06T04:41:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:23Z- waiting for 1730868087.16, time is 1730868083.62 2024-11-06T04:41:23Z- AHRS_M2 msg: 1730868083.96 epoch sec; seqNo:841 2024-11-06T04:41:23Z- AHRS_M2 msg: 0.36 phi; 0.04 theta; 1.73 psi 2024-11-06T04:41:23Z- handleDepth()- Depth_Keller msg: 1730868083.96 epoch sec; seqNo:908; depth 52.17 2024-11-06T04:41:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868083.97 epoch sec; seqNo:908; 4061681.68 north; 594235.39 east; 36.70 lat; -121.95 lon; 52.17 depth; utmZone: 10 2024-11-06T04:41:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868083.16 2024-11-06T04:41:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:24Z- waiting for 1730868087.16, time is 1730868083.97 2024-11-06T04:41:24Z- AHRS_M2 msg: 1730868084.39 epoch sec; seqNo:842 2024-11-06T04:41:24Z- AHRS_M2 msg: 0.35 phi; 0.05 theta; 1.76 psi 2024-11-06T04:41:24Z- handleDepth()- Depth_Keller msg: 1730868084.41 epoch sec; seqNo:909; depth 52.14 2024-11-06T04:41:24Z- handleDVL() - DVL msg epochMillisec: 1730868084.43 2024-11-06T04:41:24Z- handleDvl() - RDI_Pathfinder msg: 1730868084.43 sec; seqNo:316; #beams:4 2024-11-06T04:41:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 57.00 , 37.78 , 45.29 , 44.63 2024-11-06T04:41:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.03 , 0.00 2024-11-06T04:41:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868084.45 epoch sec; seqNo:909; 4061681.68 north; 594235.39 east; 36.70 lat; -121.95 lon; 52.14 depth; utmZone: 10 2024-11-06T04:41:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868083.16 2024-11-06T04:41:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:24Z- waiting for 1730868087.16, time is 1730868084.45 2024-11-06T04:41:24Z- handleDepth()- Depth_Keller msg: 1730868084.78 epoch sec; seqNo:910; depth 52.11 2024-11-06T04:41:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868084.81 epoch sec; seqNo:910; 4061681.31 north; 594236.61 east; 36.70 lat; -121.95 lon; 52.11 depth; utmZone: 10 2024-11-06T04:41:24Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:41:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868083.16 2024-11-06T04:41:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:24Z- waiting for 1730868087.16, time is 1730868084.81 2024-11-06T04:41:25Z- AHRS_M2 msg: 1730868085.17 epoch sec; seqNo:843 2024-11-06T04:41:25Z- AHRS_M2 msg: 0.37 phi; 0.06 theta; 1.84 psi 2024-11-06T04:41:25Z- handleDepth()- Depth_Keller msg: 1730868085.17 epoch sec; seqNo:911; depth 52.09 2024-11-06T04:41:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868085.18 epoch sec; seqNo:911; 4061681.31 north; 594236.61 east; 36.70 lat; -121.95 lon; 52.09 depth; utmZone: 10 2024-11-06T04:41:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868083.16 2024-11-06T04:41:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:25Z- waiting for 1730868087.16, time is 1730868085.18 2024-11-06T04:41:25Z- AHRS_M2 msg: 1730868085.58 epoch sec; seqNo:844 2024-11-06T04:41:25Z- AHRS_M2 msg: 0.38 phi; 0.06 theta; 1.87 psi 2024-11-06T04:41:25Z- handleDepth()- Depth_Keller msg: 1730868085.59 epoch sec; seqNo:912; depth 52.06 2024-11-06T04:41:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868085.61 epoch sec; seqNo:912; 4061681.31 north; 594236.61 east; 36.70 lat; -121.95 lon; 52.06 depth; utmZone: 10 2024-11-06T04:41:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868083.16 2024-11-06T04:41:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:25Z- waiting for 1730868087.16, time is 1730868085.61 2024-11-06T04:41:26Z- handleDepth()- Depth_Keller msg: 1730868086.00 epoch sec; seqNo:913; depth 52.05 2024-11-06T04:41:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868086.01 epoch sec; seqNo:913; 4061681.31 north; 594236.61 east; 36.70 lat; -121.95 lon; 52.05 depth; utmZone: 10 2024-11-06T04:41:26Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:41:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868083.16 2024-11-06T04:41:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:26Z- waiting for 1730868087.16, time is 1730868086.01 2024-11-06T04:41:26Z- AHRS_M2 msg: 1730868086.41 epoch sec; seqNo:845 2024-11-06T04:41:26Z- AHRS_M2 msg: 0.39 phi; 0.07 theta; 1.94 psi 2024-11-06T04:41:26Z- handleDepth()- Depth_Keller msg: 1730868086.41 epoch sec; seqNo:914; depth 52.01 2024-11-06T04:41:26Z- handleDVL() - DVL msg epochMillisec: 1730868086.44 2024-11-06T04:41:26Z- handleDvl() - RDI_Pathfinder msg: 1730868086.44 sec; seqNo:317; #beams:4 2024-11-06T04:41:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 57.66 , 37.78 , 46.17 , 44.63 2024-11-06T04:41:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.03 , 0.01 2024-11-06T04:41:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868086.45 epoch sec; seqNo:914; 4061681.32 north; 594237.83 east; 36.70 lat; -121.95 lon; 52.01 depth; utmZone: 10 2024-11-06T04:41:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868083.16 2024-11-06T04:41:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:26Z- waiting for 1730868087.16, time is 1730868086.45 2024-11-06T04:41:26Z- AHRS_M2 msg: 1730868086.79 epoch sec; seqNo:846 2024-11-06T04:41:26Z- AHRS_M2 msg: 0.39 phi; 0.07 theta; 1.97 psi 2024-11-06T04:41:26Z- handleDepth()- Depth_Keller msg: 1730868086.80 epoch sec; seqNo:915; depth 51.99 2024-11-06T04:41:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868086.82 epoch sec; seqNo:915; 4061680.95 north; 594237.84 east; 36.70 lat; -121.95 lon; 51.99 depth; utmZone: 10 2024-11-06T04:41:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868083.16 2024-11-06T04:41:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:26Z- waiting for 1730868087.16, time is 1730868086.82 2024-11-06T04:41:27Z- AHRS_M2 msg: 1730868087.19 epoch sec; seqNo:847 2024-11-06T04:41:27Z- AHRS_M2 msg: 0.38 phi; 0.07 theta; 2.00 psi 2024-11-06T04:41:27Z- handleDepth()- Depth_Keller msg: 1730868087.19 epoch sec; seqNo:916; depth 51.96 2024-11-06T04:41:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868087.22 epoch sec; seqNo:916; 4061680.95 north; 594237.84 east; 36.70 lat; -121.95 lon; 51.96 depth; utmZone: 10 2024-11-06T04:41:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868083.16 2024-11-06T04:41:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:27Z- waiting for 1730868087.16, time is 1730868087.22 2024-11-06T04:41:27Z- LcmTrn::updateTrn() >>>> pose time 1730868087.22, meas time 1730868086.44 <<<< 2024-11-06T04:41:27Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868087.22 2024-11-06T04:41:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868086.44 sec, ping # 0; waiting for INS data more recent than 1730868083.16... 2024-11-06T04:41:27Z- TerrainNav::motionUpdate() - currEstimate = (4061681, 594237.84, 51.96) 2024-11-06T04:41:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:41:27Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:41:27Z- TNavPF::Projecting Measurements 2024-11-06T04:41:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:41:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:41:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:41:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:41:27Z- TNavFilter::4 good beams. 2024-11-06T04:41:27Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:41:27Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:41:27Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:41:27Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:41:27Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:41:27Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:41:27Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:41:27Z- TerrainMapOctree: (vehN, vehE ) = (4061681.09, 594237.37). 2024-11-06T04:41:27Z- TerrainMapOctree: Min Distance = 70.06. 2024-11-06T04:41:27Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:41:27Z- Forcing Subcloud Comparison 2024-11-06T04:41:27Z- Weighting particles with subcloud comparison 2024-11-06T04:41:27Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:27Z- meanExpectedMeasDiff: -1.754591 delta_rms_squared: 2.428857 alpha: 0.991833 2024-11-06T04:41:27Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:27Z- meanExpectedMeasDiff: -0.302349 delta_rms_squared: 0.939257 alpha: 0.979151 2024-11-06T04:41:27Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:27Z- meanExpectedMeasDiff: -0.819285 delta_rms_squared: 0.850399 alpha: 0.977022 2024-11-06T04:41:27Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:27Z- meanExpectedMeasDiff: -0.417685 delta_rms_squared: 1.264969 alpha: 0.984435 2024-11-06T04:41:27Z- Subcloud NIS: 0.171216 2024-11-06T04:41:27Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:41:27Z- TNavPF:: sumWeights = 0.582749 2024-11-06T04:41:27Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:41:27Z- TNavPF::First Term of Map Covariance Matrix is 0.88 2024-11-06T04:41:27Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.03 2024-11-06T04:41:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868086.44 sec. 2024-11-06T04:41:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:41:27Z- variances([0], [2], and psi_berg) = 0.143415 0.135500 0.000000 2024-11-06T04:41:27Z- publishEstimatesDecoder() - at 1730868087.22 sec 2024-11-06T04:41:27Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:41:27Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061625.05 594242.25 58.41 2024-11-06T04:41:27Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061624.89 594241.88 57.34 lat/lon: 36.70 -121.95 2024-11-06T04:41:27Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.14 0.73 0.00 2024-11-06T04:41:27Z- AHRS_M2 msg: 1730868087.62 epoch sec; seqNo:848 2024-11-06T04:41:27Z- AHRS_M2 msg: 0.37 phi; 0.07 theta; 2.03 psi 2024-11-06T04:41:27Z- handleDepth()- Depth_Keller msg: 1730868087.64 epoch sec; seqNo:917; depth 51.93 2024-11-06T04:41:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868087.66 epoch sec; seqNo:917; 4061680.95 north; 594237.84 east; 36.70 lat; -121.95 lon; 51.93 depth; utmZone: 10 2024-11-06T04:41:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868087.22 2024-11-06T04:41:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:27Z- waiting for 1730868091.22, time is 1730868087.66 2024-11-06T04:41:28Z- AHRS_M2 msg: 1730868088.00 epoch sec; seqNo:849 2024-11-06T04:41:28Z- AHRS_M2 msg: 0.36 phi; 0.08 theta; 2.06 psi 2024-11-06T04:41:28Z- handleDepth()- Depth_Keller msg: 1730868088.01 epoch sec; seqNo:918; depth 51.89 2024-11-06T04:41:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868088.02 epoch sec; seqNo:918; 4061680.57 north; 594237.84 east; 36.70 lat; -121.95 lon; 51.89 depth; utmZone: 10 2024-11-06T04:41:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868087.22 2024-11-06T04:41:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:28Z- waiting for 1730868091.22, time is 1730868088.02 2024-11-06T04:41:28Z- AHRS_M2 msg: 1730868088.42 epoch sec; seqNo:850 2024-11-06T04:41:28Z- AHRS_M2 msg: 0.35 phi; 0.08 theta; 2.08 psi 2024-11-06T04:41:28Z- handleDepth()- Depth_Keller msg: 1730868088.43 epoch sec; seqNo:919; depth 51.88 2024-11-06T04:41:28Z- handleDVL() - DVL msg epochMillisec: 1730868088.45 2024-11-06T04:41:28Z- handleDvl() - RDI_Pathfinder msg: 1730868088.45 sec; seqNo:318; #beams:4 2024-11-06T04:41:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 57.00 , 37.56 , 46.39 , 44.18 2024-11-06T04:41:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.02 , 0.00 2024-11-06T04:41:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868088.47 epoch sec; seqNo:919; 4061680.58 north; 594239.06 east; 36.70 lat; -121.95 lon; 51.88 depth; utmZone: 10 2024-11-06T04:41:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868087.22 2024-11-06T04:41:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:28Z- waiting for 1730868091.22, time is 1730868088.47 2024-11-06T04:41:28Z- AHRS_M2 msg: 1730868088.80 epoch sec; seqNo:851 2024-11-06T04:41:28Z- AHRS_M2 msg: 0.34 phi; 0.08 theta; 2.10 psi 2024-11-06T04:41:28Z- handleDepth()- Depth_Keller msg: 1730868088.81 epoch sec; seqNo:920; depth 51.83 2024-11-06T04:41:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868088.85 epoch sec; seqNo:920; 4061680.58 north; 594239.06 east; 36.70 lat; -121.95 lon; 51.83 depth; utmZone: 10 2024-11-06T04:41:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868087.22 2024-11-06T04:41:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:28Z- waiting for 1730868091.22, time is 1730868088.85 2024-11-06T04:41:29Z- AHRS_M2 msg: 1730868089.21 epoch sec; seqNo:852 2024-11-06T04:41:29Z- AHRS_M2 msg: 0.33 phi; 0.09 theta; 2.12 psi 2024-11-06T04:41:29Z- handleDepth()- Depth_Keller msg: 1730868089.21 epoch sec; seqNo:921; depth 51.82 2024-11-06T04:41:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868089.22 epoch sec; seqNo:921; 4061680.20 north; 594239.07 east; 36.70 lat; -121.95 lon; 51.82 depth; utmZone: 10 2024-11-06T04:41:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868087.22 2024-11-06T04:41:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:29Z- waiting for 1730868091.22, time is 1730868089.22 2024-11-06T04:41:29Z- AHRS_M2 msg: 1730868089.62 epoch sec; seqNo:853 2024-11-06T04:41:29Z- AHRS_M2 msg: 0.31 phi; 0.09 theta; 2.14 psi 2024-11-06T04:41:29Z- handleDepth()- Depth_Keller msg: 1730868089.62 epoch sec; seqNo:922; depth 51.78 2024-11-06T04:41:29Z- handleDVL() - DVL msg epochMillisec: 1730868089.63 2024-11-06T04:41:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:41:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:41:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:41:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:41:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:41:29Z- handleDvl() - RDI_Pathfinder msg: 1730868089.63 sec; seqNo:319; #beams:0 2024-11-06T04:41:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:41:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.02 , 0.00 2024-11-06T04:41:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868089.64 epoch sec; seqNo:922; 4061680.20 north; 594239.07 east; 36.70 lat; -121.95 lon; 51.78 depth; utmZone: 10 2024-11-06T04:41:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868087.22 2024-11-06T04:41:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:29Z- waiting for 1730868091.22, time is 1730868089.64 2024-11-06T04:41:30Z- handleDepth()- Depth_Keller msg: 1730868090.04 epoch sec; seqNo:923; depth 51.75 2024-11-06T04:41:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868090.05 epoch sec; seqNo:923; 4061679.84 north; 594240.29 east; 36.70 lat; -121.95 lon; 51.75 depth; utmZone: 10 2024-11-06T04:41:30Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:41:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868087.22 2024-11-06T04:41:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:30Z- waiting for 1730868091.22, time is 1730868090.05 2024-11-06T04:41:30Z- AHRS_M2 msg: 1730868090.42 epoch sec; seqNo:854 2024-11-06T04:41:30Z- AHRS_M2 msg: 0.28 phi; 0.11 theta; 2.17 psi 2024-11-06T04:41:30Z- handleDepth()- Depth_Keller msg: 1730868090.44 epoch sec; seqNo:924; depth 51.72 2024-11-06T04:41:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868090.45 epoch sec; seqNo:924; 4061679.84 north; 594240.29 east; 36.70 lat; -121.95 lon; 51.72 depth; utmZone: 10 2024-11-06T04:41:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868087.22 2024-11-06T04:41:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:30Z- waiting for 1730868091.22, time is 1730868090.45 2024-11-06T04:41:30Z- AHRS_M2 msg: 1730868090.83 epoch sec; seqNo:855 2024-11-06T04:41:30Z- AHRS_M2 msg: 0.27 phi; 0.11 theta; 2.18 psi 2024-11-06T04:41:30Z- handleDepth()- Depth_Keller msg: 1730868090.84 epoch sec; seqNo:925; depth 51.69 2024-11-06T04:41:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868090.85 epoch sec; seqNo:925; 4061679.46 north; 594240.30 east; 36.70 lat; -121.95 lon; 51.69 depth; utmZone: 10 2024-11-06T04:41:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868087.22 2024-11-06T04:41:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:30Z- waiting for 1730868091.22, time is 1730868090.85 2024-11-06T04:41:31Z- AHRS_M2 msg: 1730868091.23 epoch sec; seqNo:856 2024-11-06T04:41:31Z- AHRS_M2 msg: 0.26 phi; 0.12 theta; 2.19 psi 2024-11-06T04:41:31Z- handleDepth()- Depth_Keller msg: 1730868091.23 epoch sec; seqNo:926; depth 51.65 2024-11-06T04:41:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868091.24 epoch sec; seqNo:926; 4061679.46 north; 594240.30 east; 36.70 lat; -121.95 lon; 51.65 depth; utmZone: 10 2024-11-06T04:41:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868087.22 2024-11-06T04:41:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:31Z- waiting for 1730868091.22, time is 1730868091.24 2024-11-06T04:41:31Z- LcmTrn::updateTrn() >>>> pose time 1730868091.24, meas time 1730868089.63 <<<< 2024-11-06T04:41:31Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868091.24 2024-11-06T04:41:31Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:41:31Z- TerrainNav::motionUpdate() - currEstimate = (4061679, 594240.30, 51.65) 2024-11-06T04:41:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:41:31Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:41:31Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:41:31Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:41:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:41:31Z- variances([0], [2], and psi_berg) = 0.144534 0.137021 0.000000 2024-11-06T04:41:31Z- publishEstimatesDecoder() - at 1730868091.24 sec 2024-11-06T04:41:31Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:41:31Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061623.54 594244.68 58.16 2024-11-06T04:41:31Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061623.40 594244.33 57.03 lat/lon: 36.70 -121.94 2024-11-06T04:41:31Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.14 0.14 0.97 0.00 2024-11-06T04:41:31Z- AHRS_M2 msg: 1730868091.63 epoch sec; seqNo:857 2024-11-06T04:41:31Z- AHRS_M2 msg: 0.25 phi; 0.13 theta; 2.20 psi 2024-11-06T04:41:31Z- handleDepth()- Depth_Keller msg: 1730868091.64 epoch sec; seqNo:927; depth 51.60 2024-11-06T04:41:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868091.65 epoch sec; seqNo:927; 4061679.46 north; 594240.30 east; 36.70 lat; -121.95 lon; 51.60 depth; utmZone: 10 2024-11-06T04:41:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868091.24 2024-11-06T04:41:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:31Z- waiting for 1730868095.24, time is 1730868091.65 2024-11-06T04:41:32Z- AHRS_M2 msg: 1730868092.03 epoch sec; seqNo:858 2024-11-06T04:41:32Z- AHRS_M2 msg: 0.24 phi; 0.14 theta; 2.21 psi 2024-11-06T04:41:32Z- handleDepth()- Depth_Keller msg: 1730868092.04 epoch sec; seqNo:928; depth 51.57 2024-11-06T04:41:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868092.06 epoch sec; seqNo:928; 4061679.09 north; 594241.52 east; 36.70 lat; -121.95 lon; 51.57 depth; utmZone: 10 2024-11-06T04:41:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868091.24 2024-11-06T04:41:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:32Z- waiting for 1730868095.24, time is 1730868092.06 2024-11-06T04:41:32Z- AHRS_M2 msg: 1730868092.44 epoch sec; seqNo:859 2024-11-06T04:41:32Z- AHRS_M2 msg: 0.23 phi; 0.14 theta; 2.22 psi 2024-11-06T04:41:32Z- handleDepth()- Depth_Keller msg: 1730868092.45 epoch sec; seqNo:929; depth 51.53 2024-11-06T04:41:32Z- handleDVL() - DVL msg epochMillisec: 1730868092.48 2024-11-06T04:41:32Z- handleDvl() - RDI_Pathfinder msg: 1730868092.48 sec; seqNo:320; #beams:4 2024-11-06T04:41:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.17 , 39.36 , 46.90 , 41.60 2024-11-06T04:41:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.00 , 0.04 2024-11-06T04:41:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868092.49 epoch sec; seqNo:929; 4061679.09 north; 594241.52 east; 36.70 lat; -121.95 lon; 51.53 depth; utmZone: 10 2024-11-06T04:41:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868091.24 2024-11-06T04:41:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:32Z- waiting for 1730868095.24, time is 1730868092.49 2024-11-06T04:41:32Z- AHRS_M2 msg: 1730868092.86 epoch sec; seqNo:860 2024-11-06T04:41:32Z- AHRS_M2 msg: 0.22 phi; 0.15 theta; 2.22 psi 2024-11-06T04:41:32Z- handleDepth()- Depth_Keller msg: 1730868092.87 epoch sec; seqNo:930; depth 51.50 2024-11-06T04:41:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868092.90 epoch sec; seqNo:930; 4061678.71 north; 594241.52 east; 36.70 lat; -121.95 lon; 51.50 depth; utmZone: 10 2024-11-06T04:41:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868091.24 2024-11-06T04:41:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:33Z- waiting for 1730868095.24, time is 1730868092.90 2024-11-06T04:41:33Z- AHRS_M2 msg: 1730868093.25 epoch sec; seqNo:861 2024-11-06T04:41:33Z- AHRS_M2 msg: 0.21 phi; 0.16 theta; 2.23 psi 2024-11-06T04:41:33Z- handleDepth()- Depth_Keller msg: 1730868093.26 epoch sec; seqNo:931; depth 51.45 2024-11-06T04:41:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868093.26 epoch sec; seqNo:931; 4061678.71 north; 594241.52 east; 36.70 lat; -121.95 lon; 51.45 depth; utmZone: 10 2024-11-06T04:41:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868091.24 2024-11-06T04:41:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:33Z- waiting for 1730868095.24, time is 1730868093.26 2024-11-06T04:41:33Z- AHRS_M2 msg: 1730868093.65 epoch sec; seqNo:862 2024-11-06T04:41:33Z- AHRS_M2 msg: 0.20 phi; 0.17 theta; 2.24 psi 2024-11-06T04:41:33Z- handleDepth()- Depth_Keller msg: 1730868093.66 epoch sec; seqNo:932; depth 51.40 2024-11-06T04:41:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868093.68 epoch sec; seqNo:932; 4061678.33 north; 594241.53 east; 36.70 lat; -121.95 lon; 51.40 depth; utmZone: 10 2024-11-06T04:41:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868091.24 2024-11-06T04:41:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:33Z- waiting for 1730868095.24, time is 1730868093.68 2024-11-06T04:41:34Z- handleDepth()- Depth_Keller msg: 1730868094.08 epoch sec; seqNo:933; depth 51.34 2024-11-06T04:41:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868094.08 epoch sec; seqNo:933; 4061678.35 north; 594242.75 east; 36.70 lat; -121.95 lon; 51.34 depth; utmZone: 10 2024-11-06T04:41:34Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:41:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868091.24 2024-11-06T04:41:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:34Z- waiting for 1730868095.24, time is 1730868094.08 2024-11-06T04:41:34Z- AHRS_M2 msg: 1730868094.46 epoch sec; seqNo:863 2024-11-06T04:41:34Z- AHRS_M2 msg: 0.19 phi; 0.19 theta; 2.26 psi 2024-11-06T04:41:34Z- handleDepth()- Depth_Keller msg: 1730868094.47 epoch sec; seqNo:934; depth 51.28 2024-11-06T04:41:34Z- handleDVL() - DVL msg epochMillisec: 1730868094.50 2024-11-06T04:41:34Z- handleDvl() - RDI_Pathfinder msg: 1730868094.50 sec; seqNo:321; #beams:4 2024-11-06T04:41:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.14 , 40.37 , 48.33 , 40.78 2024-11-06T04:41:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , 0.01 , 0.04 2024-11-06T04:41:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868094.51 epoch sec; seqNo:934; 4061677.97 north; 594242.75 east; 36.70 lat; -121.95 lon; 51.28 depth; utmZone: 10 2024-11-06T04:41:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868091.24 2024-11-06T04:41:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:34Z- waiting for 1730868095.24, time is 1730868094.51 2024-11-06T04:41:34Z- AHRS_M2 msg: 1730868094.88 epoch sec; seqNo:864 2024-11-06T04:41:34Z- AHRS_M2 msg: 0.19 phi; 0.20 theta; 2.26 psi 2024-11-06T04:41:34Z- handleDepth()- Depth_Keller msg: 1730868094.89 epoch sec; seqNo:935; depth 51.22 2024-11-06T04:41:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868094.90 epoch sec; seqNo:935; 4061677.59 north; 594242.76 east; 36.70 lat; -121.95 lon; 51.22 depth; utmZone: 10 2024-11-06T04:41:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868091.24 2024-11-06T04:41:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:35Z- waiting for 1730868095.24, time is 1730868094.90 2024-11-06T04:41:35Z- AHRS_M2 msg: 1730868095.29 epoch sec; seqNo:865 2024-11-06T04:41:35Z- AHRS_M2 msg: 0.19 phi; 0.20 theta; 2.27 psi 2024-11-06T04:41:35Z- handleDepth()- Depth_Keller msg: 1730868095.29 epoch sec; seqNo:936; depth 51.15 2024-11-06T04:41:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868095.30 epoch sec; seqNo:936; 4061677.59 north; 594242.76 east; 36.70 lat; -121.95 lon; 51.15 depth; utmZone: 10 2024-11-06T04:41:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868091.24 2024-11-06T04:41:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:35Z- waiting for 1730868095.24, time is 1730868095.30 2024-11-06T04:41:35Z- LcmTrn::updateTrn() >>>> pose time 1730868095.30, meas time 1730868094.50 <<<< 2024-11-06T04:41:35Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868095.30 2024-11-06T04:41:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868094.50 sec, ping # 0; waiting for INS data more recent than 1730868091.24... 2024-11-06T04:41:35Z- TerrainNav::motionUpdate() - currEstimate = (4061678, 594242.76, 51.15) 2024-11-06T04:41:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:41:35Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:41:35Z- TNavPF::Projecting Measurements 2024-11-06T04:41:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:41:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:41:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:41:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:41:35Z- TNavFilter::4 good beams. 2024-11-06T04:41:35Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:41:35Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:41:35Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:41:35Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:41:35Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:41:35Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:41:35Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:41:35Z- TerrainMapOctree: (vehN, vehE ) = (4061677.96, 594242.27). 2024-11-06T04:41:35Z- TerrainMapOctree: Min Distance = 72.45. 2024-11-06T04:41:35Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:41:35Z- Forcing Subcloud Comparison 2024-11-06T04:41:35Z- Weighting particles with subcloud comparison 2024-11-06T04:41:35Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:35Z- meanExpectedMeasDiff: -1.413325 delta_rms_squared: 2.290632 alpha: 0.991344 2024-11-06T04:41:35Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:35Z- meanExpectedMeasDiff: -0.350708 delta_rms_squared: 1.407929 alpha: 0.985994 2024-11-06T04:41:35Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:35Z- meanExpectedMeasDiff: -1.139461 delta_rms_squared: 1.758633 alpha: 0.988755 2024-11-06T04:41:35Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:35Z- meanExpectedMeasDiff: -0.368469 delta_rms_squared: 1.437413 alpha: 0.986277 2024-11-06T04:41:35Z- Subcloud NIS: 0.177181 2024-11-06T04:41:35Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:41:35Z- TNavPF:: sumWeights = 0.544844 2024-11-06T04:41:35Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:41:35Z- TNavPF::First Term of Map Covariance Matrix is 0.64 2024-11-06T04:41:35Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T04:41:35Z- TerrainNav::Resampling particle filter... 2024-11-06T04:41:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868094.50 sec. 2024-11-06T04:41:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:41:35Z- variances([0], [2], and psi_berg) = 0.146339 0.138982 0.000000 2024-11-06T04:41:35Z- publishEstimatesDecoder() - at 1730868095.30 sec 2024-11-06T04:41:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:41:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061621.73 594246.82 58.17 2024-11-06T04:41:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061621.52 594246.79 56.94 lat/lon: 36.70 -121.94 2024-11-06T04:41:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.14 0.79 0.00 2024-11-06T04:41:35Z- AHRS_M2 msg: 1730868095.73 epoch sec; seqNo:866 2024-11-06T04:41:35Z- AHRS_M2 msg: 0.19 phi; 0.20 theta; 2.28 psi 2024-11-06T04:41:35Z- handleDepth()- Depth_Keller msg: 1730868095.74 epoch sec; seqNo:937; depth 51.08 2024-11-06T04:41:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868095.76 epoch sec; seqNo:937; 4061677.21 north; 594242.76 east; 36.70 lat; -121.95 lon; 51.08 depth; utmZone: 10 2024-11-06T04:41:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868095.30 2024-11-06T04:41:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:35Z- waiting for 1730868099.30, time is 1730868095.76 2024-11-06T04:41:36Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:41:36Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:41:36Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:41:36Z- AHRS_M2 msg: 1730868096.10 epoch sec; seqNo:867 2024-11-06T04:41:36Z- AHRS_M2 msg: 0.19 phi; 0.20 theta; 2.28 psi 2024-11-06T04:41:36Z- handleDepth()- Depth_Keller msg: 1730868096.13 epoch sec; seqNo:938; depth 51.01 2024-11-06T04:41:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868096.14 epoch sec; seqNo:938; 4061677.22 north; 594243.98 east; 36.70 lat; -121.94 lon; 51.01 depth; utmZone: 10 2024-11-06T04:41:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868095.30 2024-11-06T04:41:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:36Z- waiting for 1730868099.30, time is 1730868096.14 2024-11-06T04:41:36Z- AHRS_M2 msg: 1730868096.51 epoch sec; seqNo:868 2024-11-06T04:41:36Z- AHRS_M2 msg: 0.17 phi; 0.20 theta; 2.28 psi 2024-11-06T04:41:36Z- handleDepth()- Depth_Keller msg: 1730868096.51 epoch sec; seqNo:939; depth 50.92 2024-11-06T04:41:36Z- handleDVL() - DVL msg epochMillisec: 1730868096.54 2024-11-06T04:41:36Z- handleDvl() - RDI_Pathfinder msg: 1730868096.54 sec; seqNo:322; #beams:4 2024-11-06T04:41:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.35 , 40.99 , 49.96 , 40.78 2024-11-06T04:41:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.02 2024-11-06T04:41:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868096.55 epoch sec; seqNo:939; 4061676.85 north; 594243.99 east; 36.70 lat; -121.94 lon; 50.92 depth; utmZone: 10 2024-11-06T04:41:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868095.30 2024-11-06T04:41:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:36Z- waiting for 1730868099.30, time is 1730868096.55 2024-11-06T04:41:36Z- AHRS_M2 msg: 1730868096.91 epoch sec; seqNo:869 2024-11-06T04:41:36Z- AHRS_M2 msg: 0.16 phi; 0.20 theta; 2.28 psi 2024-11-06T04:41:36Z- handleDepth()- Depth_Keller msg: 1730868096.91 epoch sec; seqNo:940; depth 50.85 2024-11-06T04:41:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868096.93 epoch sec; seqNo:940; 4061676.85 north; 594243.99 east; 36.70 lat; -121.94 lon; 50.85 depth; utmZone: 10 2024-11-06T04:41:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868095.30 2024-11-06T04:41:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:37Z- waiting for 1730868099.30, time is 1730868096.93 2024-11-06T04:41:37Z- AHRS_M2 msg: 1730868097.32 epoch sec; seqNo:870 2024-11-06T04:41:37Z- AHRS_M2 msg: 0.15 phi; 0.20 theta; 2.28 psi 2024-11-06T04:41:37Z- handleDepth()- Depth_Keller msg: 1730868097.33 epoch sec; seqNo:941; depth 50.78 2024-11-06T04:41:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868097.34 epoch sec; seqNo:941; 4061676.47 north; 594243.99 east; 36.70 lat; -121.94 lon; 50.78 depth; utmZone: 10 2024-11-06T04:41:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868095.30 2024-11-06T04:41:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:37Z- waiting for 1730868099.30, time is 1730868097.34 2024-11-06T04:41:37Z- AHRS_M2 msg: 1730868097.72 epoch sec; seqNo:871 2024-11-06T04:41:37Z- AHRS_M2 msg: 0.15 phi; 0.19 theta; 2.28 psi 2024-11-06T04:41:37Z- handleDepth()- Depth_Keller msg: 1730868097.72 epoch sec; seqNo:942; depth 50.70 2024-11-06T04:41:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868097.74 epoch sec; seqNo:942; 4061676.10 north; 594245.21 east; 36.70 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T04:41:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868095.30 2024-11-06T04:41:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:37Z- waiting for 1730868099.30, time is 1730868097.74 2024-11-06T04:41:38Z- AHRS_M2 msg: 1730868098.12 epoch sec; seqNo:872 2024-11-06T04:41:38Z- AHRS_M2 msg: 0.14 phi; 0.19 theta; 2.28 psi 2024-11-06T04:41:38Z- handleDepth()- Depth_Keller msg: 1730868098.13 epoch sec; seqNo:943; depth 50.62 2024-11-06T04:41:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868098.14 epoch sec; seqNo:943; 4061676.10 north; 594245.21 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T04:41:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868095.30 2024-11-06T04:41:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:38Z- waiting for 1730868099.30, time is 1730868098.14 2024-11-06T04:41:38Z- handleDepth()- Depth_Keller msg: 1730868098.55 epoch sec; seqNo:944; depth 50.54 2024-11-06T04:41:38Z- handleDVL() - DVL msg epochMillisec: 1730868098.57 2024-11-06T04:41:38Z- handleDvl() - RDI_Pathfinder msg: 1730868098.57 sec; seqNo:323; #beams:4 2024-11-06T04:41:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.12 , 42.01 , 49.75 , 40.78 2024-11-06T04:41:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.01 2024-11-06T04:41:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868098.59 epoch sec; seqNo:944; 4061675.72 north; 594245.22 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T04:41:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868095.30 2024-11-06T04:41:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:38Z- waiting for 1730868099.30, time is 1730868098.59 2024-11-06T04:41:38Z- AHRS_M2 msg: 1730868098.93 epoch sec; seqNo:873 2024-11-06T04:41:38Z- AHRS_M2 msg: 0.14 phi; 0.19 theta; 2.28 psi 2024-11-06T04:41:38Z- handleDepth()- Depth_Keller msg: 1730868098.95 epoch sec; seqNo:945; depth 50.44 2024-11-06T04:41:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868098.96 epoch sec; seqNo:945; 4061675.72 north; 594245.22 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T04:41:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868095.30 2024-11-06T04:41:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:39Z- waiting for 1730868099.30, time is 1730868098.96 2024-11-06T04:41:39Z- AHRS_M2 msg: 1730868099.34 epoch sec; seqNo:874 2024-11-06T04:41:39Z- AHRS_M2 msg: 0.13 phi; 0.19 theta; 2.28 psi 2024-11-06T04:41:39Z- handleDepth()- Depth_Keller msg: 1730868099.35 epoch sec; seqNo:946; depth 50.38 2024-11-06T04:41:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868099.36 epoch sec; seqNo:946; 4061675.34 north; 594245.22 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:41:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868095.30 2024-11-06T04:41:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:39Z- waiting for 1730868099.30, time is 1730868099.36 2024-11-06T04:41:39Z- LcmTrn::updateTrn() >>>> pose time 1730868099.36, meas time 1730868098.57 <<<< 2024-11-06T04:41:39Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868099.36 2024-11-06T04:41:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868098.57 sec, ping # 0; waiting for INS data more recent than 1730868095.30... 2024-11-06T04:41:39Z- TerrainNav::motionUpdate() - currEstimate = (4061675, 594245.22, 50.38) 2024-11-06T04:41:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:41:39Z- TerrainNav:: SubcloudNIS is 0.18, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:41:39Z- TNavPF::Projecting Measurements 2024-11-06T04:41:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:41:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:41:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:41:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:41:39Z- TNavFilter::4 good beams. 2024-11-06T04:41:39Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:41:39Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:41:39Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:41:39Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:41:39Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:41:39Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:41:39Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:41:39Z- TerrainMapOctree: (vehN, vehE ) = (4061675.78, 594244.74). 2024-11-06T04:41:39Z- TerrainMapOctree: Min Distance = 74.41. 2024-11-06T04:41:39Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:41:39Z- Forcing Subcloud Comparison 2024-11-06T04:41:39Z- Weighting particles with subcloud comparison 2024-11-06T04:41:39Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:39Z- meanExpectedMeasDiff: -0.340082 delta_rms_squared: 1.527548 alpha: 0.987076 2024-11-06T04:41:39Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:39Z- meanExpectedMeasDiff: -0.007668 delta_rms_squared: 1.241245 alpha: 0.984143 2024-11-06T04:41:39Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:39Z- meanExpectedMeasDiff: -0.250176 delta_rms_squared: 1.858634 alpha: 0.989354 2024-11-06T04:41:39Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:39Z- meanExpectedMeasDiff: -0.206256 delta_rms_squared: 1.180912 alpha: 0.983346 2024-11-06T04:41:39Z- Subcloud NIS: 0.170874 2024-11-06T04:41:39Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:41:39Z- TNavPF:: sumWeights = 0.669559 2024-11-06T04:41:39Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:41:39Z- TNavPF::First Term of Map Covariance Matrix is 0.52 2024-11-06T04:41:39Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T04:41:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868098.57 sec. 2024-11-06T04:41:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:41:39Z- variances([0], [2], and psi_berg) = 0.147760 0.141636 0.000000 2024-11-06T04:41:39Z- publishEstimatesDecoder() - at 1730868099.36 sec 2024-11-06T04:41:39Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:41:39Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061619.79 594250.45 56.27 2024-11-06T04:41:39Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061619.27 594249.26 56.27 lat/lon: 36.70 -121.94 2024-11-06T04:41:39Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.14 0.74 0.00 2024-11-06T04:41:39Z- AHRS_M2 msg: 1730868099.77 epoch sec; seqNo:875 2024-11-06T04:41:39Z- AHRS_M2 msg: 0.13 phi; 0.19 theta; 2.28 psi 2024-11-06T04:41:39Z- handleDepth()- Depth_Keller msg: 1730868099.77 epoch sec; seqNo:947; depth 50.30 2024-11-06T04:41:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868099.79 epoch sec; seqNo:947; 4061675.36 north; 594246.44 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:41:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868099.36 2024-11-06T04:41:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:39Z- waiting for 1730868103.36, time is 1730868099.79 2024-11-06T04:41:40Z- AHRS_M2 msg: 1730868100.14 epoch sec; seqNo:876 2024-11-06T04:41:40Z- AHRS_M2 msg: 0.13 phi; 0.18 theta; 2.28 psi 2024-11-06T04:41:40Z- handleDepth()- Depth_Keller msg: 1730868100.15 epoch sec; seqNo:948; depth 50.20 2024-11-06T04:41:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868100.16 epoch sec; seqNo:948; 4061674.98 north; 594246.45 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:41:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868099.36 2024-11-06T04:41:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:40Z- waiting for 1730868103.36, time is 1730868100.16 2024-11-06T04:41:40Z- AHRS_M2 msg: 1730868100.55 epoch sec; seqNo:877 2024-11-06T04:41:40Z- AHRS_M2 msg: 0.13 phi; 0.18 theta; 2.28 psi 2024-11-06T04:41:40Z- handleDepth()- Depth_Keller msg: 1730868100.56 epoch sec; seqNo:949; depth 50.14 2024-11-06T04:41:40Z- handleDVL() - DVL msg epochMillisec: 1730868100.58 2024-11-06T04:41:40Z- handleDvl() - RDI_Pathfinder msg: 1730868100.58 sec; seqNo:324; #beams:4 2024-11-06T04:41:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.51 , 42.62 , 49.96 , 41.19 2024-11-06T04:41:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.03 2024-11-06T04:41:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868100.59 epoch sec; seqNo:949; 4061674.60 north; 594246.45 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:41:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868099.36 2024-11-06T04:41:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:40Z- waiting for 1730868103.36, time is 1730868100.59 2024-11-06T04:41:40Z- AHRS_M2 msg: 1730868100.95 epoch sec; seqNo:878 2024-11-06T04:41:40Z- AHRS_M2 msg: 0.13 phi; 0.18 theta; 2.28 psi 2024-11-06T04:41:40Z- handleDepth()- Depth_Keller msg: 1730868100.96 epoch sec; seqNo:950; depth 50.06 2024-11-06T04:41:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868100.98 epoch sec; seqNo:950; 4061674.60 north; 594246.45 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T04:41:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868099.36 2024-11-06T04:41:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:41Z- waiting for 1730868103.36, time is 1730868100.98 2024-11-06T04:41:41Z- AHRS_M2 msg: 1730868101.36 epoch sec; seqNo:879 2024-11-06T04:41:41Z- AHRS_M2 msg: 0.12 phi; 0.18 theta; 2.28 psi 2024-11-06T04:41:41Z- handleDepth()- Depth_Keller msg: 1730868101.36 epoch sec; seqNo:951; depth 49.96 2024-11-06T04:41:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868101.37 epoch sec; seqNo:951; 4061674.22 north; 594246.46 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:41:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868099.36 2024-11-06T04:41:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:41Z- waiting for 1730868103.36, time is 1730868101.37 2024-11-06T04:41:41Z- AHRS_M2 msg: 1730868101.77 epoch sec; seqNo:880 2024-11-06T04:41:41Z- AHRS_M2 msg: 0.12 phi; 0.17 theta; 2.28 psi 2024-11-06T04:41:41Z- handleDepth()- Depth_Keller msg: 1730868101.78 epoch sec; seqNo:952; depth 49.89 2024-11-06T04:41:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868101.79 epoch sec; seqNo:952; 4061674.23 north; 594247.68 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:41:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868099.36 2024-11-06T04:41:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:41Z- waiting for 1730868103.36, time is 1730868101.79 2024-11-06T04:41:42Z- AHRS_M2 msg: 1730868102.17 epoch sec; seqNo:881 2024-11-06T04:41:42Z- AHRS_M2 msg: 0.12 phi; 0.17 theta; 2.28 psi 2024-11-06T04:41:42Z- handleDepth()- Depth_Keller msg: 1730868102.19 epoch sec; seqNo:953; depth 49.80 2024-11-06T04:41:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868102.20 epoch sec; seqNo:953; 4061673.85 north; 594247.68 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T04:41:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868099.36 2024-11-06T04:41:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:42Z- waiting for 1730868103.36, time is 1730868102.20 2024-11-06T04:41:42Z- handleDepth()- Depth_Keller msg: 1730868102.59 epoch sec; seqNo:954; depth 49.72 2024-11-06T04:41:42Z- handleDVL() - DVL msg epochMillisec: 1730868102.61 2024-11-06T04:41:42Z- handleDvl() - RDI_Pathfinder msg: 1730868102.61 sec; seqNo:325; #beams:4 2024-11-06T04:41:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 42.85 , 49.70 , 41.53 2024-11-06T04:41:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.01 2024-11-06T04:41:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868102.62 epoch sec; seqNo:954; 4061673.85 north; 594247.68 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:41:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868099.36 2024-11-06T04:41:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:42Z- waiting for 1730868103.36, time is 1730868102.62 2024-11-06T04:41:42Z- AHRS_M2 msg: 1730868102.97 epoch sec; seqNo:882 2024-11-06T04:41:42Z- AHRS_M2 msg: 0.12 phi; 0.17 theta; 2.28 psi 2024-11-06T04:41:42Z- handleDepth()- Depth_Keller msg: 1730868102.98 epoch sec; seqNo:955; depth 49.63 2024-11-06T04:41:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868103.00 epoch sec; seqNo:955; 4061673.48 north; 594247.68 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:41:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868099.36 2024-11-06T04:41:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:43Z- waiting for 1730868103.36, time is 1730868103.00 2024-11-06T04:41:43Z- AHRS_M2 msg: 1730868103.38 epoch sec; seqNo:883 2024-11-06T04:41:43Z- AHRS_M2 msg: 0.12 phi; 0.16 theta; 2.28 psi 2024-11-06T04:41:43Z- handleDepth()- Depth_Keller msg: 1730868103.38 epoch sec; seqNo:956; depth 49.56 2024-11-06T04:41:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868103.39 epoch sec; seqNo:956; 4061673.11 north; 594248.91 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:41:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868099.36 2024-11-06T04:41:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:43Z- waiting for 1730868103.36, time is 1730868103.39 2024-11-06T04:41:43Z- LcmTrn::updateTrn() >>>> pose time 1730868103.39, meas time 1730868102.61 <<<< 2024-11-06T04:41:43Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868103.39 2024-11-06T04:41:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868102.61 sec, ping # 0; waiting for INS data more recent than 1730868099.36... 2024-11-06T04:41:43Z- TerrainNav::motionUpdate() - currEstimate = (4061673, 594248.91, 49.56) 2024-11-06T04:41:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:41:43Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:41:43Z- TNavPF::Projecting Measurements 2024-11-06T04:41:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:41:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:41:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:41:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:41:43Z- TNavFilter::4 good beams. 2024-11-06T04:41:43Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:41:43Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:41:43Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:41:43Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:41:43Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:41:43Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:41:43Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:41:43Z- TerrainMapOctree: (vehN, vehE ) = (4061673.54, 594248.19). 2024-11-06T04:41:43Z- TerrainMapOctree: Min Distance = 76.48. 2024-11-06T04:41:43Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:41:43Z- Forcing Subcloud Comparison 2024-11-06T04:41:43Z- Weighting particles with subcloud comparison 2024-11-06T04:41:43Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:43Z- meanExpectedMeasDiff: 0.511557 delta_rms_squared: 1.585025 alpha: 0.987539 2024-11-06T04:41:43Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:43Z- meanExpectedMeasDiff: -0.126322 delta_rms_squared: 1.183759 alpha: 0.983385 2024-11-06T04:41:43Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:43Z- meanExpectedMeasDiff: -0.125018 delta_rms_squared: 1.679689 alpha: 0.988233 2024-11-06T04:41:43Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:43Z- meanExpectedMeasDiff: -0.061228 delta_rms_squared: 1.234024 alpha: 0.984051 2024-11-06T04:41:43Z- Subcloud NIS: 0.168839 2024-11-06T04:41:43Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:41:43Z- TNavPF:: sumWeights = 0.664897 2024-11-06T04:41:43Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:41:43Z- TNavPF::First Term of Map Covariance Matrix is 0.54 2024-11-06T04:41:43Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T04:41:43Z- TerrainNav::Resampling particle filter... 2024-11-06T04:41:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868102.61 sec. 2024-11-06T04:41:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:41:43Z- variances([0], [2], and psi_berg) = 0.147901 0.143735 0.000000 2024-11-06T04:41:43Z- publishEstimatesDecoder() - at 1730868103.39 sec 2024-11-06T04:41:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:41:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061617.20 594253.08 56.52 2024-11-06T04:41:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061617.04 594252.94 55.43 lat/lon: 36.70 -121.94 2024-11-06T04:41:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.14 0.74 0.00 2024-11-06T04:41:43Z- handleDepth()- Depth_Keller msg: 1730868103.82 epoch sec; seqNo:957; depth 49.48 2024-11-06T04:41:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868103.83 epoch sec; seqNo:957; 4061673.11 north; 594248.91 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T04:41:43Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:41:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868103.39 2024-11-06T04:41:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:43Z- waiting for 1730868107.39, time is 1730868103.83 2024-11-06T04:41:44Z- AHRS_M2 msg: 1730868104.19 epoch sec; seqNo:884 2024-11-06T04:41:44Z- AHRS_M2 msg: 0.12 phi; 0.15 theta; 2.28 psi 2024-11-06T04:41:44Z- handleDepth()- Depth_Keller msg: 1730868104.19 epoch sec; seqNo:958; depth 49.40 2024-11-06T04:41:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868104.20 epoch sec; seqNo:958; 4061672.73 north; 594248.91 east; 36.70 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T04:41:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868103.39 2024-11-06T04:41:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:44Z- waiting for 1730868107.39, time is 1730868104.20 2024-11-06T04:41:44Z- AHRS_M2 msg: 1730868104.59 epoch sec; seqNo:885 2024-11-06T04:41:44Z- AHRS_M2 msg: 0.12 phi; 0.15 theta; 2.28 psi 2024-11-06T04:41:44Z- handleDepth()- Depth_Keller msg: 1730868104.59 epoch sec; seqNo:959; depth 49.33 2024-11-06T04:41:44Z- handleDVL() - DVL msg epochMillisec: 1730868104.61 2024-11-06T04:41:44Z- handleDvl() - RDI_Pathfinder msg: 1730868104.61 sec; seqNo:326; #beams:4 2024-11-06T04:41:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 43.08 , 49.26 , 41.97 2024-11-06T04:41:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.00 , -0.01 2024-11-06T04:41:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868104.64 epoch sec; seqNo:959; 4061672.73 north; 594248.91 east; 36.70 lat; -121.94 lon; 49.33 depth; utmZone: 10 2024-11-06T04:41:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868103.39 2024-11-06T04:41:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:44Z- waiting for 1730868107.39, time is 1730868104.64 2024-11-06T04:41:45Z- AHRS_M2 msg: 1730868104.99 epoch sec; seqNo:886 2024-11-06T04:41:45Z- AHRS_M2 msg: 0.12 phi; 0.14 theta; 2.28 psi 2024-11-06T04:41:45Z- handleDepth()- Depth_Keller msg: 1730868105.00 epoch sec; seqNo:960; depth 49.26 2024-11-06T04:41:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868105.01 epoch sec; seqNo:960; 4061672.35 north; 594248.92 east; 36.70 lat; -121.94 lon; 49.26 depth; utmZone: 10 2024-11-06T04:41:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868103.39 2024-11-06T04:41:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:45Z- waiting for 1730868107.39, time is 1730868105.01 2024-11-06T04:41:45Z- AHRS_M2 msg: 1730868105.39 epoch sec; seqNo:887 2024-11-06T04:41:45Z- AHRS_M2 msg: 0.12 phi; 0.14 theta; 2.28 psi 2024-11-06T04:41:45Z- handleDepth()- Depth_Keller msg: 1730868105.40 epoch sec; seqNo:961; depth 49.19 2024-11-06T04:41:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868105.42 epoch sec; seqNo:961; 4061671.99 north; 594250.14 east; 36.70 lat; -121.94 lon; 49.19 depth; utmZone: 10 2024-11-06T04:41:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868103.39 2024-11-06T04:41:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:45Z- waiting for 1730868107.39, time is 1730868105.42 2024-11-06T04:41:45Z- AHRS_M2 msg: 1730868105.80 epoch sec; seqNo:888 2024-11-06T04:41:45Z- AHRS_M2 msg: 0.12 phi; 0.13 theta; 2.28 psi 2024-11-06T04:41:45Z- handleDepth()- Depth_Keller msg: 1730868105.80 epoch sec; seqNo:962; depth 49.13 2024-11-06T04:41:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868105.82 epoch sec; seqNo:962; 4061671.99 north; 594250.14 east; 36.70 lat; -121.94 lon; 49.13 depth; utmZone: 10 2024-11-06T04:41:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868103.39 2024-11-06T04:41:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:45Z- waiting for 1730868107.39, time is 1730868105.82 2024-11-06T04:41:46Z- AHRS_M2 msg: 1730868106.20 epoch sec; seqNo:889 2024-11-06T04:41:46Z- AHRS_M2 msg: 0.12 phi; 0.13 theta; 2.27 psi 2024-11-06T04:41:46Z- handleDepth()- Depth_Keller msg: 1730868106.21 epoch sec; seqNo:963; depth 49.06 2024-11-06T04:41:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868106.22 epoch sec; seqNo:963; 4061671.61 north; 594250.15 east; 36.70 lat; -121.94 lon; 49.06 depth; utmZone: 10 2024-11-06T04:41:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868103.39 2024-11-06T04:41:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:46Z- waiting for 1730868107.39, time is 1730868106.22 2024-11-06T04:41:46Z- AHRS_M2 msg: 1730868106.61 epoch sec; seqNo:890 2024-11-06T04:41:46Z- AHRS_M2 msg: 0.12 phi; 0.12 theta; 2.27 psi 2024-11-06T04:41:46Z- handleDepth()- Depth_Keller msg: 1730868106.61 epoch sec; seqNo:964; depth 48.99 2024-11-06T04:41:46Z- handleDVL() - DVL msg epochMillisec: 1730868106.64 2024-11-06T04:41:46Z- handleDvl() - RDI_Pathfinder msg: 1730868106.64 sec; seqNo:327; #beams:4 2024-11-06T04:41:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.94 , 43.30 , 48.60 , 42.63 2024-11-06T04:41:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.00 , 0.00 2024-11-06T04:41:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868106.65 epoch sec; seqNo:964; 4061671.61 north; 594250.15 east; 36.70 lat; -121.94 lon; 48.99 depth; utmZone: 10 2024-11-06T04:41:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868103.39 2024-11-06T04:41:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:46Z- waiting for 1730868107.39, time is 1730868106.65 2024-11-06T04:41:47Z- AHRS_M2 msg: 1730868107.01 epoch sec; seqNo:891 2024-11-06T04:41:47Z- AHRS_M2 msg: 0.12 phi; 0.11 theta; 2.27 psi 2024-11-06T04:41:47Z- handleDepth()- Depth_Keller msg: 1730868107.03 epoch sec; seqNo:965; depth 48.93 2024-11-06T04:41:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868107.04 epoch sec; seqNo:965; 4061671.23 north; 594250.15 east; 36.70 lat; -121.94 lon; 48.93 depth; utmZone: 10 2024-11-06T04:41:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868103.39 2024-11-06T04:41:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:47Z- waiting for 1730868107.39, time is 1730868107.04 2024-11-06T04:41:47Z- AHRS_M2 msg: 1730868107.41 epoch sec; seqNo:892 2024-11-06T04:41:47Z- AHRS_M2 msg: 0.11 phi; 0.11 theta; 2.26 psi 2024-11-06T04:41:47Z- handleDepth()- Depth_Keller msg: 1730868107.42 epoch sec; seqNo:966; depth 48.87 2024-11-06T04:41:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868107.43 epoch sec; seqNo:966; 4061671.24 north; 594251.37 east; 36.70 lat; -121.94 lon; 48.87 depth; utmZone: 10 2024-11-06T04:41:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868103.39 2024-11-06T04:41:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:47Z- waiting for 1730868107.39, time is 1730868107.43 2024-11-06T04:41:47Z- LcmTrn::updateTrn() >>>> pose time 1730868107.43, meas time 1730868106.64 <<<< 2024-11-06T04:41:47Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868107.43 2024-11-06T04:41:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868106.64 sec, ping # 0; waiting for INS data more recent than 1730868103.39... 2024-11-06T04:41:47Z- TerrainNav::motionUpdate() - currEstimate = (4061671, 594251.37, 48.87) 2024-11-06T04:41:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:41:47Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:41:47Z- TNavPF::Projecting Measurements 2024-11-06T04:41:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:41:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:41:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:41:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:41:47Z- TNavFilter::4 good beams. 2024-11-06T04:41:47Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:41:47Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:41:47Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:41:47Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:41:47Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:41:47Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:41:47Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:41:47Z- TerrainMapOctree: (vehN, vehE ) = (4061671.61, 594250.89). 2024-11-06T04:41:47Z- TerrainMapOctree: Min Distance = 78.40. 2024-11-06T04:41:47Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:41:47Z- Forcing Subcloud Comparison 2024-11-06T04:41:47Z- Weighting particles with subcloud comparison 2024-11-06T04:41:47Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:47Z- meanExpectedMeasDiff: 0.561710 delta_rms_squared: 1.519962 alpha: 0.987013 2024-11-06T04:41:47Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:47Z- meanExpectedMeasDiff: -0.429176 delta_rms_squared: 1.165729 alpha: 0.983133 2024-11-06T04:41:47Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:47Z- meanExpectedMeasDiff: -0.240619 delta_rms_squared: 1.533359 alpha: 0.987125 2024-11-06T04:41:47Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:47Z- meanExpectedMeasDiff: -0.454563 delta_rms_squared: 1.757032 alpha: 0.988745 2024-11-06T04:41:47Z- Subcloud NIS: 0.170789 2024-11-06T04:41:47Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:41:47Z- TNavPF:: sumWeights = 0.618412 2024-11-06T04:41:47Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:41:47Z- TNavPF::First Term of Map Covariance Matrix is 0.49 2024-11-06T04:41:47Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T04:41:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868106.64 sec. 2024-11-06T04:41:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:41:47Z- variances([0], [2], and psi_berg) = 0.152120 0.148976 0.000000 2024-11-06T04:41:47Z- publishEstimatesDecoder() - at 1730868107.43 sec 2024-11-06T04:41:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:41:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061616.03 594255.33 55.12 2024-11-06T04:41:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061615.18 594255.41 54.78 lat/lon: 36.70 -121.94 2024-11-06T04:41:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.15 0.72 0.00 2024-11-06T04:41:47Z- handleDepth()- Depth_Keller msg: 1730868107.85 epoch sec; seqNo:967; depth 48.81 2024-11-06T04:41:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868107.88 epoch sec; seqNo:967; 4061670.86 north; 594251.37 east; 36.70 lat; -121.94 lon; 48.81 depth; utmZone: 10 2024-11-06T04:41:48Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:41:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868107.43 2024-11-06T04:41:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:48Z- waiting for 1730868111.43, time is 1730868107.88 2024-11-06T04:41:48Z- handleDepth()- Depth_Keller msg: 1730868108.24 epoch sec; seqNo:968; depth 48.75 2024-11-06T04:41:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868108.26 epoch sec; seqNo:968; 4061670.86 north; 594251.37 east; 36.70 lat; -121.94 lon; 48.75 depth; utmZone: 10 2024-11-06T04:41:48Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:41:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868107.43 2024-11-06T04:41:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:48Z- waiting for 1730868111.43, time is 1730868108.26 2024-11-06T04:41:48Z- AHRS_M2 msg: 1730868108.63 epoch sec; seqNo:893 2024-11-06T04:41:48Z- AHRS_M2 msg: 0.09 phi; 0.09 theta; 2.24 psi 2024-11-06T04:41:48Z- handleDepth()- Depth_Keller msg: 1730868108.64 epoch sec; seqNo:969; depth 48.70 2024-11-06T04:41:48Z- handleDVL() - DVL msg epochMillisec: 1730868108.67 2024-11-06T04:41:48Z- handleDvl() - RDI_Pathfinder msg: 1730868108.67 sec; seqNo:328; #beams:4 2024-11-06T04:41:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.27 , 43.29 , 47.71 , 43.29 2024-11-06T04:41:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.00 , 0.07 2024-11-06T04:41:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868108.68 epoch sec; seqNo:969; 4061670.49 north; 594251.38 east; 36.70 lat; -121.94 lon; 48.70 depth; utmZone: 10 2024-11-06T04:41:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868107.43 2024-11-06T04:41:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:48Z- waiting for 1730868111.43, time is 1730868108.68 2024-11-06T04:41:49Z- handleDepth()- Depth_Keller msg: 1730868109.05 epoch sec; seqNo:970; depth 48.64 2024-11-06T04:41:49Z- handleDVL() - DVL msg epochMillisec: 1730868109.06 2024-11-06T04:41:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:41:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:41:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:41:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:41:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:41:49Z- handleDvl() - RDI_Pathfinder msg: 1730868109.06 sec; seqNo:329; #beams:0 2024-11-06T04:41:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:41:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.00 , 0.07 2024-11-06T04:41:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868109.08 epoch sec; seqNo:970; 4061670.49 north; 594251.38 east; 36.70 lat; -121.94 lon; 48.64 depth; utmZone: 10 2024-11-06T04:41:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868107.43 2024-11-06T04:41:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:49Z- waiting for 1730868111.43, time is 1730868109.08 2024-11-06T04:41:49Z- AHRS_M2 msg: 1730868109.43 epoch sec; seqNo:894 2024-11-06T04:41:49Z- AHRS_M2 msg: 0.08 phi; 0.08 theta; 2.23 psi 2024-11-06T04:41:49Z- handleDepth()- Depth_Keller msg: 1730868109.44 epoch sec; seqNo:971; depth 48.59 2024-11-06T04:41:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868109.45 epoch sec; seqNo:971; 4061670.12 north; 594252.60 east; 36.70 lat; -121.94 lon; 48.59 depth; utmZone: 10 2024-11-06T04:41:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868107.43 2024-11-06T04:41:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:49Z- waiting for 1730868111.43, time is 1730868109.45 2024-11-06T04:41:49Z- AHRS_M2 msg: 1730868109.85 epoch sec; seqNo:895 2024-11-06T04:41:49Z- AHRS_M2 msg: 0.07 phi; 0.07 theta; 2.22 psi 2024-11-06T04:41:49Z- handleDepth()- Depth_Keller msg: 1730868109.86 epoch sec; seqNo:972; depth 48.55 2024-11-06T04:41:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868109.87 epoch sec; seqNo:972; 4061670.12 north; 594252.60 east; 36.70 lat; -121.94 lon; 48.55 depth; utmZone: 10 2024-11-06T04:41:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868107.43 2024-11-06T04:41:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:50Z- waiting for 1730868111.43, time is 1730868109.87 2024-11-06T04:41:50Z- AHRS_M2 msg: 1730868110.27 epoch sec; seqNo:896 2024-11-06T04:41:50Z- AHRS_M2 msg: 0.07 phi; 0.06 theta; 2.21 psi 2024-11-06T04:41:50Z- handleDepth()- Depth_Keller msg: 1730868110.27 epoch sec; seqNo:973; depth 48.50 2024-11-06T04:41:50Z- handleDVL() - DVL msg epochMillisec: 1730868110.27 2024-11-06T04:41:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:41:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:41:50Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:41:50Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:41:50Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:41:50Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:41:50Z- handleDvl() - RDI_Pathfinder msg: 1730868110.27 sec; seqNo:330; #beams:0 2024-11-06T04:41:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:41:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , 0.00 , 0.07 2024-11-06T04:41:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868110.28 epoch sec; seqNo:973; 4061669.74 north; 594252.61 east; 36.70 lat; -121.94 lon; 48.50 depth; utmZone: 10 2024-11-06T04:41:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868107.43 2024-11-06T04:41:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:50Z- waiting for 1730868111.43, time is 1730868110.28 2024-11-06T04:41:50Z- AHRS_M2 msg: 1730868110.65 epoch sec; seqNo:897 2024-11-06T04:41:50Z- AHRS_M2 msg: 0.07 phi; 0.06 theta; 2.20 psi 2024-11-06T04:41:50Z- handleDepth()- Depth_Keller msg: 1730868110.66 epoch sec; seqNo:974; depth 48.46 2024-11-06T04:41:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868110.69 epoch sec; seqNo:974; 4061669.74 north; 594252.61 east; 36.70 lat; -121.94 lon; 48.46 depth; utmZone: 10 2024-11-06T04:41:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868107.43 2024-11-06T04:41:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:50Z- waiting for 1730868111.43, time is 1730868110.69 2024-11-06T04:41:51Z- AHRS_M2 msg: 1730868111.05 epoch sec; seqNo:898 2024-11-06T04:41:51Z- AHRS_M2 msg: 0.07 phi; 0.05 theta; 2.19 psi 2024-11-06T04:41:51Z- handleDepth()- Depth_Keller msg: 1730868111.06 epoch sec; seqNo:975; depth 48.41 2024-11-06T04:41:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868111.07 epoch sec; seqNo:975; 4061669.38 north; 594253.83 east; 36.70 lat; -121.94 lon; 48.41 depth; utmZone: 10 2024-11-06T04:41:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868107.43 2024-11-06T04:41:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:51Z- waiting for 1730868111.43, time is 1730868111.07 2024-11-06T04:41:51Z- AHRS_M2 msg: 1730868111.46 epoch sec; seqNo:899 2024-11-06T04:41:51Z- AHRS_M2 msg: 0.07 phi; 0.05 theta; 2.19 psi 2024-11-06T04:41:51Z- handleDepth()- Depth_Keller msg: 1730868111.46 epoch sec; seqNo:976; depth 48.37 2024-11-06T04:41:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868111.47 epoch sec; seqNo:976; 4061669.38 north; 594253.83 east; 36.70 lat; -121.94 lon; 48.37 depth; utmZone: 10 2024-11-06T04:41:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868107.43 2024-11-06T04:41:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:51Z- waiting for 1730868111.43, time is 1730868111.47 2024-11-06T04:41:51Z- LcmTrn::updateTrn() >>>> pose time 1730868111.47, meas time 1730868110.27 <<<< 2024-11-06T04:41:51Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868111.47 2024-11-06T04:41:51Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:41:51Z- TerrainNav::motionUpdate() - currEstimate = (4061669, 594253.83, 48.37) 2024-11-06T04:41:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:41:51Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:41:51Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:41:51Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:41:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:41:51Z- variances([0], [2], and psi_berg) = 0.153779 0.150540 0.000000 2024-11-06T04:41:51Z- publishEstimatesDecoder() - at 1730868111.47 sec 2024-11-06T04:41:51Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:41:51Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061614.17 594257.77 54.13 2024-11-06T04:41:51Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061613.31 594257.87 54.29 lat/lon: 36.70 -121.94 2024-11-06T04:41:51Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.15 0.97 0.00 2024-11-06T04:41:51Z- AHRS_M2 msg: 1730868111.86 epoch sec; seqNo:900 2024-11-06T04:41:51Z- AHRS_M2 msg: 0.07 phi; 0.04 theta; 2.18 psi 2024-11-06T04:41:51Z- handleDepth()- Depth_Keller msg: 1730868111.87 epoch sec; seqNo:977; depth 48.33 2024-11-06T04:41:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868111.89 epoch sec; seqNo:977; 4061669.00 north; 594253.84 east; 36.70 lat; -121.94 lon; 48.33 depth; utmZone: 10 2024-11-06T04:41:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868111.47 2024-11-06T04:41:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:52Z- waiting for 1730868115.47, time is 1730868111.89 2024-11-06T04:41:52Z- handleDepth()- Depth_Keller msg: 1730868112.28 epoch sec; seqNo:978; depth 48.30 2024-11-06T04:41:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868112.30 epoch sec; seqNo:978; 4061669.00 north; 594253.84 east; 36.70 lat; -121.94 lon; 48.30 depth; utmZone: 10 2024-11-06T04:41:52Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:41:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868111.47 2024-11-06T04:41:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:52Z- waiting for 1730868115.47, time is 1730868112.30 2024-11-06T04:41:52Z- AHRS_M2 msg: 1730868112.67 epoch sec; seqNo:901 2024-11-06T04:41:52Z- AHRS_M2 msg: 0.07 phi; 0.04 theta; 2.17 psi 2024-11-06T04:41:52Z- handleDepth()- Depth_Keller msg: 1730868112.67 epoch sec; seqNo:979; depth 48.26 2024-11-06T04:41:52Z- handleDVL() - DVL msg epochMillisec: 1730868112.69 2024-11-06T04:41:52Z- handleDvl() - RDI_Pathfinder msg: 1730868112.69 sec; seqNo:331; #beams:4 2024-11-06T04:41:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.83 , 44.62 , 46.38 , 45.06 2024-11-06T04:41:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.06 , 0.11 2024-11-06T04:41:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868112.72 epoch sec; seqNo:979; 4061668.62 north; 594253.84 east; 36.70 lat; -121.94 lon; 48.26 depth; utmZone: 10 2024-11-06T04:41:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868111.47 2024-11-06T04:41:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:52Z- waiting for 1730868115.47, time is 1730868112.72 2024-11-06T04:41:53Z- handleDepth()- Depth_Keller msg: 1730868113.10 epoch sec; seqNo:980; depth 48.22 2024-11-06T04:41:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868113.11 epoch sec; seqNo:980; 4061668.63 north; 594255.06 east; 36.70 lat; -121.94 lon; 48.22 depth; utmZone: 10 2024-11-06T04:41:53Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:41:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868111.47 2024-11-06T04:41:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:53Z- waiting for 1730868115.47, time is 1730868113.11 2024-11-06T04:41:53Z- AHRS_M2 msg: 1730868113.47 epoch sec; seqNo:902 2024-11-06T04:41:53Z- AHRS_M2 msg: 0.07 phi; 0.03 theta; 2.16 psi 2024-11-06T04:41:53Z- handleDepth()- Depth_Keller msg: 1730868113.48 epoch sec; seqNo:981; depth 48.19 2024-11-06T04:41:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868113.49 epoch sec; seqNo:981; 4061668.25 north; 594255.06 east; 36.70 lat; -121.94 lon; 48.19 depth; utmZone: 10 2024-11-06T04:41:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868111.47 2024-11-06T04:41:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:53Z- waiting for 1730868115.47, time is 1730868113.49 2024-11-06T04:41:53Z- AHRS_M2 msg: 1730868113.89 epoch sec; seqNo:903 2024-11-06T04:41:53Z- AHRS_M2 msg: 0.07 phi; 0.02 theta; 2.15 psi 2024-11-06T04:41:53Z- handleDepth()- Depth_Keller msg: 1730868113.90 epoch sec; seqNo:982; depth 48.15 2024-11-06T04:41:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868113.91 epoch sec; seqNo:982; 4061668.25 north; 594255.06 east; 36.70 lat; -121.94 lon; 48.15 depth; utmZone: 10 2024-11-06T04:41:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868111.47 2024-11-06T04:41:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:54Z- waiting for 1730868115.47, time is 1730868113.91 2024-11-06T04:41:54Z- AHRS_M2 msg: 1730868114.30 epoch sec; seqNo:904 2024-11-06T04:41:54Z- AHRS_M2 msg: 0.07 phi; 0.02 theta; 2.15 psi 2024-11-06T04:41:54Z- handleDepth()- Depth_Keller msg: 1730868114.30 epoch sec; seqNo:983; depth 48.10 2024-11-06T04:41:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868114.31 epoch sec; seqNo:983; 4061667.89 north; 594256.29 east; 36.70 lat; -121.94 lon; 48.10 depth; utmZone: 10 2024-11-06T04:41:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868111.47 2024-11-06T04:41:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:54Z- waiting for 1730868115.47, time is 1730868114.31 2024-11-06T04:41:54Z- AHRS_M2 msg: 1730868114.69 epoch sec; seqNo:905 2024-11-06T04:41:54Z- AHRS_M2 msg: 0.06 phi; 0.01 theta; 2.14 psi 2024-11-06T04:41:54Z- handleDepth()- Depth_Keller msg: 1730868114.69 epoch sec; seqNo:984; depth 48.08 2024-11-06T04:41:54Z- handleDVL() - DVL msg epochMillisec: 1730868114.72 2024-11-06T04:41:54Z- handleDvl() - RDI_Pathfinder msg: 1730868114.72 sec; seqNo:332; #beams:4 2024-11-06T04:41:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.83 , 44.84 , 45.94 , 45.94 2024-11-06T04:41:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.04 , 0.02 2024-11-06T04:41:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868114.73 epoch sec; seqNo:984; 4061667.89 north; 594256.29 east; 36.70 lat; -121.94 lon; 48.08 depth; utmZone: 10 2024-11-06T04:41:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868111.47 2024-11-06T04:41:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:54Z- waiting for 1730868115.47, time is 1730868114.73 2024-11-06T04:41:55Z- AHRS_M2 msg: 1730868115.10 epoch sec; seqNo:906 2024-11-06T04:41:55Z- AHRS_M2 msg: 0.06 phi; 0.01 theta; 2.14 psi 2024-11-06T04:41:55Z- handleDepth()- Depth_Keller msg: 1730868115.10 epoch sec; seqNo:985; depth 48.05 2024-11-06T04:41:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868115.14 epoch sec; seqNo:985; 4061667.51 north; 594256.29 east; 36.70 lat; -121.94 lon; 48.05 depth; utmZone: 10 2024-11-06T04:41:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868111.47 2024-11-06T04:41:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:55Z- waiting for 1730868115.47, time is 1730868115.14 2024-11-06T04:41:55Z- AHRS_M2 msg: 1730868115.49 epoch sec; seqNo:907 2024-11-06T04:41:55Z- AHRS_M2 msg: 0.05 phi; 0.01 theta; 2.13 psi 2024-11-06T04:41:55Z- handleDepth()- Depth_Keller msg: 1730868115.51 epoch sec; seqNo:986; depth 48.02 2024-11-06T04:41:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868115.52 epoch sec; seqNo:986; 4061667.51 north; 594256.29 east; 36.70 lat; -121.94 lon; 48.02 depth; utmZone: 10 2024-11-06T04:41:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868111.47 2024-11-06T04:41:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:55Z- waiting for 1730868115.47, time is 1730868115.52 2024-11-06T04:41:55Z- LcmTrn::updateTrn() >>>> pose time 1730868115.52, meas time 1730868114.72 <<<< 2024-11-06T04:41:55Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868115.52 2024-11-06T04:41:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868114.72 sec, ping # 0; waiting for INS data more recent than 1730868111.47... 2024-11-06T04:41:55Z- TerrainNav::motionUpdate() - currEstimate = (4061668, 594256.29, 48.02) 2024-11-06T04:41:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:41:55Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:41:55Z- TNavPF::Projecting Measurements 2024-11-06T04:41:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:41:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:41:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:41:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:41:55Z- TNavFilter::4 good beams. 2024-11-06T04:41:55Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:41:55Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:41:55Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:41:55Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:41:55Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:41:55Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:41:55Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:41:55Z- TerrainMapOctree: (vehN, vehE ) = (4061667.88, 594255.81). 2024-11-06T04:41:55Z- TerrainMapOctree: Min Distance = 82.33. 2024-11-06T04:41:55Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:41:55Z- Forcing Subcloud Comparison 2024-11-06T04:41:55Z- Weighting particles with subcloud comparison 2024-11-06T04:41:55Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:55Z- meanExpectedMeasDiff: 0.543882 delta_rms_squared: 1.705685 alpha: 0.988410 2024-11-06T04:41:55Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:55Z- meanExpectedMeasDiff: 0.421493 delta_rms_squared: 1.533148 alpha: 0.987123 2024-11-06T04:41:55Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:55Z- meanExpectedMeasDiff: -0.354156 delta_rms_squared: 1.666456 alpha: 0.988141 2024-11-06T04:41:55Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:55Z- meanExpectedMeasDiff: -0.689029 delta_rms_squared: 1.654341 alpha: 0.988055 2024-11-06T04:41:55Z- Subcloud NIS: 0.175000 2024-11-06T04:41:55Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:41:55Z- TNavPF:: sumWeights = 0.594505 2024-11-06T04:41:55Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:41:55Z- TNavPF::First Term of Map Covariance Matrix is 0.49 2024-11-06T04:41:55Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T04:41:55Z- TerrainNav::Resampling particle filter... 2024-11-06T04:41:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868114.72 sec. 2024-11-06T04:41:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:41:55Z- variances([0], [2], and psi_berg) = 0.154119 0.152972 0.000000 2024-11-06T04:41:55Z- publishEstimatesDecoder() - at 1730868115.52 sec 2024-11-06T04:41:55Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:41:55Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061611.60 594260.46 54.70 2024-11-06T04:41:55Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061611.44 594260.33 53.95 lat/lon: 36.70 -121.94 2024-11-06T04:41:55Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.15 0.15 0.79 0.00 2024-11-06T04:41:55Z- AHRS_M2 msg: 1730868115.91 epoch sec; seqNo:908 2024-11-06T04:41:55Z- AHRS_M2 msg: 0.05 phi; 0.00 theta; 2.12 psi 2024-11-06T04:41:55Z- handleDepth()- Depth_Keller msg: 1730868115.93 epoch sec; seqNo:987; depth 47.99 2024-11-06T04:41:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868115.94 epoch sec; seqNo:987; 4061667.14 north; 594257.52 east; 36.70 lat; -121.94 lon; 47.99 depth; utmZone: 10 2024-11-06T04:41:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868115.52 2024-11-06T04:41:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:56Z- waiting for 1730868119.52, time is 1730868115.94 2024-11-06T04:41:56Z- AHRS_M2 msg: 1730868116.30 epoch sec; seqNo:909 2024-11-06T04:41:56Z- AHRS_M2 msg: 0.05 phi; -0.00 theta; 2.11 psi 2024-11-06T04:41:56Z- handleDepth()- Depth_Keller msg: 1730868116.31 epoch sec; seqNo:988; depth 47.95 2024-11-06T04:41:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868116.32 epoch sec; seqNo:988; 4061667.14 north; 594257.52 east; 36.70 lat; -121.94 lon; 47.95 depth; utmZone: 10 2024-11-06T04:41:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868115.52 2024-11-06T04:41:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:56Z- waiting for 1730868119.52, time is 1730868116.32 2024-11-06T04:41:56Z- AHRS_M2 msg: 1730868116.71 epoch sec; seqNo:910 2024-11-06T04:41:56Z- AHRS_M2 msg: 0.04 phi; -0.00 theta; 2.09 psi 2024-11-06T04:41:56Z- handleDepth()- Depth_Keller msg: 1730868116.71 epoch sec; seqNo:989; depth 47.93 2024-11-06T04:41:56Z- handleDVL() - DVL msg epochMillisec: 1730868116.73 2024-11-06T04:41:56Z- handleDvl() - RDI_Pathfinder msg: 1730868116.73 sec; seqNo:333; #beams:4 2024-11-06T04:41:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.16 , 45.06 , 45.50 , 46.16 2024-11-06T04:41:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , -0.06 2024-11-06T04:41:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868116.76 epoch sec; seqNo:989; 4061666.76 north; 594257.52 east; 36.70 lat; -121.94 lon; 47.93 depth; utmZone: 10 2024-11-06T04:41:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868115.52 2024-11-06T04:41:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:56Z- waiting for 1730868119.52, time is 1730868116.76 2024-11-06T04:41:57Z- AHRS_M2 msg: 1730868117.11 epoch sec; seqNo:911 2024-11-06T04:41:57Z- AHRS_M2 msg: 0.04 phi; -0.01 theta; 2.08 psi 2024-11-06T04:41:57Z- handleDepth()- Depth_Keller msg: 1730868117.13 epoch sec; seqNo:990; depth 47.90 2024-11-06T04:41:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868117.15 epoch sec; seqNo:990; 4061666.76 north; 594257.52 east; 36.70 lat; -121.94 lon; 47.90 depth; utmZone: 10 2024-11-06T04:41:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868115.52 2024-11-06T04:41:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:57Z- waiting for 1730868119.52, time is 1730868117.15 2024-11-06T04:41:57Z- handleDepth()- Depth_Keller msg: 1730868117.53 epoch sec; seqNo:991; depth 47.85 2024-11-06T04:41:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868117.54 epoch sec; seqNo:991; 4061666.40 north; 594258.75 east; 36.70 lat; -121.94 lon; 47.85 depth; utmZone: 10 2024-11-06T04:41:57Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:41:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868115.52 2024-11-06T04:41:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:57Z- waiting for 1730868119.52, time is 1730868117.54 2024-11-06T04:41:57Z- AHRS_M2 msg: 1730868117.92 epoch sec; seqNo:912 2024-11-06T04:41:57Z- AHRS_M2 msg: 0.04 phi; -0.01 theta; 2.06 psi 2024-11-06T04:41:57Z- handleDepth()- Depth_Keller msg: 1730868117.93 epoch sec; seqNo:992; depth 47.85 2024-11-06T04:41:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868117.94 epoch sec; seqNo:992; 4061666.40 north; 594258.75 east; 36.70 lat; -121.94 lon; 47.85 depth; utmZone: 10 2024-11-06T04:41:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868115.52 2024-11-06T04:41:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:58Z- waiting for 1730868119.52, time is 1730868117.94 2024-11-06T04:41:58Z- AHRS_M2 msg: 1730868118.33 epoch sec; seqNo:913 2024-11-06T04:41:58Z- AHRS_M2 msg: 0.04 phi; -0.02 theta; 2.05 psi 2024-11-06T04:41:58Z- handleDepth()- Depth_Keller msg: 1730868118.33 epoch sec; seqNo:993; depth 47.82 2024-11-06T04:41:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868118.40 epoch sec; seqNo:993; 4061666.02 north; 594258.75 east; 36.70 lat; -121.94 lon; 47.82 depth; utmZone: 10 2024-11-06T04:41:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868115.52 2024-11-06T04:41:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:58Z- waiting for 1730868119.52, time is 1730868118.40 2024-11-06T04:41:58Z- AHRS_M2 msg: 1730868118.73 epoch sec; seqNo:914 2024-11-06T04:41:58Z- AHRS_M2 msg: 0.04 phi; -0.02 theta; 2.04 psi 2024-11-06T04:41:58Z- handleDepth()- Depth_Keller msg: 1730868118.74 epoch sec; seqNo:994; depth 47.79 2024-11-06T04:41:58Z- handleDVL() - DVL msg epochMillisec: 1730868118.76 2024-11-06T04:41:58Z- handleDvl() - RDI_Pathfinder msg: 1730868118.76 sec; seqNo:334; #beams:4 2024-11-06T04:41:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.94 , 45.28 , 44.84 , 46.38 2024-11-06T04:41:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.01 , -0.05 2024-11-06T04:41:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868118.78 epoch sec; seqNo:994; 4061666.03 north; 594259.97 east; 36.70 lat; -121.94 lon; 47.79 depth; utmZone: 10 2024-11-06T04:41:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:41:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868115.52 2024-11-06T04:41:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:58Z- waiting for 1730868119.52, time is 1730868118.78 2024-11-06T04:41:59Z- AHRS_M2 msg: 1730868119.19 epoch sec; seqNo:915 2024-11-06T04:41:59Z- AHRS_M2 msg: 0.03 phi; -0.02 theta; 2.03 psi 2024-11-06T04:41:59Z- handleDepth()- Depth_Keller msg: 1730868119.20 epoch sec; seqNo:995; depth 47.76 2024-11-06T04:41:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868119.22 epoch sec; seqNo:995; 4061666.03 north; 594259.97 east; 36.70 lat; -121.94 lon; 47.76 depth; utmZone: 10 2024-11-06T04:41:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868115.52 2024-11-06T04:41:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:59Z- waiting for 1730868119.52, time is 1730868119.22 2024-11-06T04:41:59Z- AHRS_M2 msg: 1730868119.58 epoch sec; seqNo:916 2024-11-06T04:41:59Z- AHRS_M2 msg: 0.03 phi; -0.03 theta; 2.01 psi 2024-11-06T04:41:59Z- handleDepth()- Depth_Keller msg: 1730868119.59 epoch sec; seqNo:996; depth 47.74 2024-11-06T04:41:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868119.60 epoch sec; seqNo:996; 4061665.65 north; 594259.97 east; 36.70 lat; -121.94 lon; 47.74 depth; utmZone: 10 2024-11-06T04:41:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:41:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868115.52 2024-11-06T04:41:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:41:59Z- waiting for 1730868119.52, time is 1730868119.60 2024-11-06T04:41:59Z- LcmTrn::updateTrn() >>>> pose time 1730868119.60, meas time 1730868118.76 <<<< 2024-11-06T04:41:59Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868119.60 2024-11-06T04:41:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868118.76 sec, ping # 0; waiting for INS data more recent than 1730868115.52... 2024-11-06T04:41:59Z- TerrainNav::motionUpdate() - currEstimate = (4061666, 594259.97, 47.74) 2024-11-06T04:41:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:41:59Z- TerrainNav:: SubcloudNIS is 0.18, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:41:59Z- TNavPF::Projecting Measurements 2024-11-06T04:41:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:41:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:41:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:41:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:41:59Z- TNavFilter::4 good beams. 2024-11-06T04:41:59Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:41:59Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:41:59Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:41:59Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:41:59Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:41:59Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:41:59Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:41:59Z- TerrainMapOctree: (vehN, vehE ) = (4061666.04, 594259.22). 2024-11-06T04:41:59Z- TerrainMapOctree: Min Distance = 84.47. 2024-11-06T04:41:59Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:41:59Z- Forcing Subcloud Comparison 2024-11-06T04:41:59Z- Weighting particles with subcloud comparison 2024-11-06T04:41:59Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:59Z- meanExpectedMeasDiff: -0.063660 delta_rms_squared: 1.387072 alpha: 0.985786 2024-11-06T04:41:59Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:59Z- meanExpectedMeasDiff: -1.085988 delta_rms_squared: 1.311297 alpha: 0.984977 2024-11-06T04:41:59Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:59Z- meanExpectedMeasDiff: -0.795278 delta_rms_squared: 1.334752 alpha: 0.985237 2024-11-06T04:41:59Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:41:59Z- meanExpectedMeasDiff: -1.488214 delta_rms_squared: 1.398538 alpha: 0.985901 2024-11-06T04:41:59Z- Subcloud NIS: 0.180653 2024-11-06T04:41:59Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:41:59Z- TNavPF:: sumWeights = 0.544408 2024-11-06T04:41:59Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:41:59Z- TNavPF::First Term of Map Covariance Matrix is 0.40 2024-11-06T04:41:59Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T04:41:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868118.76 sec. 2024-11-06T04:41:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:41:59Z- variances([0], [2], and psi_berg) = 0.156471 0.156158 0.000000 2024-11-06T04:41:59Z- publishEstimatesDecoder() - at 1730868119.60 sec 2024-11-06T04:41:59Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:41:59Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061609.74 594263.25 55.06 2024-11-06T04:41:59Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061609.59 594264.01 54.07 lat/lon: 36.70 -121.94 2024-11-06T04:41:59Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.16 0.75 0.00 2024-11-06T04:41:59Z- AHRS_M2 msg: 1730868119.99 epoch sec; seqNo:917 2024-11-06T04:41:59Z- AHRS_M2 msg: 0.03 phi; -0.03 theta; 2.00 psi 2024-11-06T04:42:00Z- handleDepth()- Depth_Keller msg: 1730868120.01 epoch sec; seqNo:997; depth 47.73 2024-11-06T04:42:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868120.03 epoch sec; seqNo:997; 4061665.65 north; 594259.97 east; 36.70 lat; -121.94 lon; 47.73 depth; utmZone: 10 2024-11-06T04:42:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868119.60 2024-11-06T04:42:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:00Z- waiting for 1730868123.60, time is 1730868120.03 2024-11-06T04:42:00Z- AHRS_M2 msg: 1730868120.44 epoch sec; seqNo:918 2024-11-06T04:42:00Z- AHRS_M2 msg: 0.03 phi; -0.03 theta; 1.98 psi 2024-11-06T04:42:00Z- handleDepth()- Depth_Keller msg: 1730868120.45 epoch sec; seqNo:998; depth 47.73 2024-11-06T04:42:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868120.48 epoch sec; seqNo:998; 4061665.67 north; 594261.20 east; 36.70 lat; -121.94 lon; 47.73 depth; utmZone: 10 2024-11-06T04:42:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868119.60 2024-11-06T04:42:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:00Z- waiting for 1730868123.60, time is 1730868120.48 2024-11-06T04:42:00Z- AHRS_M2 msg: 1730868120.80 epoch sec; seqNo:919 2024-11-06T04:42:00Z- AHRS_M2 msg: 0.02 phi; -0.04 theta; 1.97 psi 2024-11-06T04:42:00Z- handleDepth()- Depth_Keller msg: 1730868120.81 epoch sec; seqNo:999; depth 47.70 2024-11-06T04:42:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868120.82 epoch sec; seqNo:999; 4061665.29 north; 594261.20 east; 36.70 lat; -121.94 lon; 47.70 depth; utmZone: 10 2024-11-06T04:42:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868119.60 2024-11-06T04:42:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:00Z- waiting for 1730868123.60, time is 1730868120.82 2024-11-06T04:42:01Z- AHRS_M2 msg: 1730868121.20 epoch sec; seqNo:920 2024-11-06T04:42:01Z- AHRS_M2 msg: 0.02 phi; -0.04 theta; 1.96 psi 2024-11-06T04:42:01Z- handleDepth()- Depth_Keller msg: 1730868121.22 epoch sec; seqNo:1000; depth 47.69 2024-11-06T04:42:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868121.23 epoch sec; seqNo:1000; 4061665.29 north; 594261.20 east; 36.70 lat; -121.94 lon; 47.69 depth; utmZone: 10 2024-11-06T04:42:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868119.60 2024-11-06T04:42:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:01Z- waiting for 1730868123.60, time is 1730868121.23 2024-11-06T04:42:01Z- AHRS_M2 msg: 1730868121.60 epoch sec; seqNo:921 2024-11-06T04:42:01Z- AHRS_M2 msg: 0.02 phi; -0.05 theta; 1.94 psi 2024-11-06T04:42:01Z- handleDepth()- Depth_Keller msg: 1730868121.61 epoch sec; seqNo:1001; depth 47.70 2024-11-06T04:42:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868121.62 epoch sec; seqNo:1001; 4061665.29 north; 594261.20 east; 36.70 lat; -121.94 lon; 47.70 depth; utmZone: 10 2024-11-06T04:42:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868119.60 2024-11-06T04:42:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:01Z- waiting for 1730868123.60, time is 1730868121.62 2024-11-06T04:42:02Z- AHRS_M2 msg: 1730868122.01 epoch sec; seqNo:922 2024-11-06T04:42:02Z- AHRS_M2 msg: 0.02 phi; -0.05 theta; 1.93 psi 2024-11-06T04:42:02Z- handleDepth()- Depth_Keller msg: 1730868122.01 epoch sec; seqNo:1002; depth 47.69 2024-11-06T04:42:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868122.02 epoch sec; seqNo:1002; 4061664.91 north; 594261.20 east; 36.70 lat; -121.94 lon; 47.69 depth; utmZone: 10 2024-11-06T04:42:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868119.60 2024-11-06T04:42:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:02Z- waiting for 1730868123.60, time is 1730868122.02 2024-11-06T04:42:02Z- AHRS_M2 msg: 1730868122.42 epoch sec; seqNo:923 2024-11-06T04:42:02Z- AHRS_M2 msg: 0.02 phi; -0.05 theta; 1.91 psi 2024-11-06T04:42:02Z- handleDepth()- Depth_Keller msg: 1730868122.43 epoch sec; seqNo:1003; depth 47.68 2024-11-06T04:42:02Z- handleDVL() - DVL msg epochMillisec: 1730868122.45 2024-11-06T04:42:02Z- handleDvl() - RDI_Pathfinder msg: 1730868122.45 sec; seqNo:335; #beams:4 2024-11-06T04:42:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.50 , 45.72 , 43.96 , 47.71 2024-11-06T04:42:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.04 , 0.12 2024-11-06T04:42:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868122.49 epoch sec; seqNo:1003; 4061664.92 north; 594262.42 east; 36.70 lat; -121.94 lon; 47.68 depth; utmZone: 10 2024-11-06T04:42:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868119.60 2024-11-06T04:42:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:02Z- waiting for 1730868123.60, time is 1730868122.49 2024-11-06T04:42:02Z- AHRS_M2 msg: 1730868122.82 epoch sec; seqNo:924 2024-11-06T04:42:02Z- AHRS_M2 msg: 0.02 phi; -0.06 theta; 1.90 psi 2024-11-06T04:42:02Z- handleDepth()- Depth_Keller msg: 1730868122.83 epoch sec; seqNo:1004; depth 47.67 2024-11-06T04:42:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868122.85 epoch sec; seqNo:1004; 4061664.92 north; 594262.42 east; 36.70 lat; -121.94 lon; 47.67 depth; utmZone: 10 2024-11-06T04:42:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868119.60 2024-11-06T04:42:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:02Z- waiting for 1730868123.60, time is 1730868122.85 2024-11-06T04:42:03Z- AHRS_M2 msg: 1730868123.22 epoch sec; seqNo:925 2024-11-06T04:42:03Z- AHRS_M2 msg: 0.02 phi; -0.06 theta; 1.89 psi 2024-11-06T04:42:03Z- handleDepth()- Depth_Keller msg: 1730868123.22 epoch sec; seqNo:1005; depth 47.67 2024-11-06T04:42:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868123.23 epoch sec; seqNo:1005; 4061664.92 north; 594262.42 east; 36.70 lat; -121.94 lon; 47.67 depth; utmZone: 10 2024-11-06T04:42:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868119.60 2024-11-06T04:42:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:03Z- waiting for 1730868123.60, time is 1730868123.23 2024-11-06T04:42:03Z- AHRS_M2 msg: 1730868123.62 epoch sec; seqNo:926 2024-11-06T04:42:03Z- AHRS_M2 msg: 0.02 phi; -0.06 theta; 1.88 psi 2024-11-06T04:42:03Z- handleDepth()- Depth_Keller msg: 1730868123.63 epoch sec; seqNo:1006; depth 47.67 2024-11-06T04:42:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868123.64 epoch sec; seqNo:1006; 4061664.92 north; 594262.42 east; 36.70 lat; -121.94 lon; 47.67 depth; utmZone: 10 2024-11-06T04:42:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868119.60 2024-11-06T04:42:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:03Z- waiting for 1730868123.60, time is 1730868123.64 2024-11-06T04:42:03Z- LcmTrn::updateTrn() >>>> pose time 1730868123.64, meas time 1730868122.45 <<<< 2024-11-06T04:42:03Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868123.64 2024-11-06T04:42:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868122.45 sec, ping # 0; waiting for INS data more recent than 1730868119.60... 2024-11-06T04:42:03Z- TerrainNav::motionUpdate() - currEstimate = (4061665, 594262.42, 47.67) 2024-11-06T04:42:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:42:03Z- TerrainNav:: SubcloudNIS is 0.18, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:42:03Z- TNavPF::Projecting Measurements 2024-11-06T04:42:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:42:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:42:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:42:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:42:03Z- TNavFilter::4 good beams. 2024-11-06T04:42:03Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:42:03Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:42:03Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:42:03Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:42:03Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:42:03Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:42:03Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:42:03Z- TerrainMapOctree: (vehN, vehE ) = (4061665.14, 594261.71). 2024-11-06T04:42:03Z- TerrainMapOctree: Min Distance = 85.67. 2024-11-06T04:42:03Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:42:03Z- Forcing Subcloud Comparison 2024-11-06T04:42:03Z- Weighting particles with subcloud comparison 2024-11-06T04:42:03Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:03Z- meanExpectedMeasDiff: -0.904125 delta_rms_squared: 1.626592 alpha: 0.987854 2024-11-06T04:42:03Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:03Z- meanExpectedMeasDiff: -0.696624 delta_rms_squared: 1.276842 alpha: 0.984578 2024-11-06T04:42:03Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:03Z- meanExpectedMeasDiff: -0.409509 delta_rms_squared: 1.113725 alpha: 0.982359 2024-11-06T04:42:03Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:03Z- meanExpectedMeasDiff: -0.806698 delta_rms_squared: 1.396621 alpha: 0.985882 2024-11-06T04:42:03Z- Subcloud NIS: 0.179351 2024-11-06T04:42:03Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:42:03Z- TNavPF:: sumWeights = 0.622184 2024-11-06T04:42:03Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:42:03Z- TNavPF::First Term of Map Covariance Matrix is 0.56 2024-11-06T04:42:03Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T04:42:03Z- TerrainNav::Resampling particle filter... 2024-11-06T04:42:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868122.45 sec. 2024-11-06T04:42:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:42:03Z- variances([0], [2], and psi_berg) = 0.158732 0.159680 0.000000 2024-11-06T04:42:03Z- publishEstimatesDecoder() - at 1730868123.64 sec 2024-11-06T04:42:03Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:42:03Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061609.06 594266.59 53.80 2024-11-06T04:42:03Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061608.86 594266.45 54.31 lat/lon: 36.70 -121.94 2024-11-06T04:42:03Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.16 0.73 0.00 2024-11-06T04:42:04Z- AHRS_M2 msg: 1730868124.04 epoch sec; seqNo:927 2024-11-06T04:42:04Z- AHRS_M2 msg: 0.02 phi; -0.07 theta; 1.87 psi 2024-11-06T04:42:04Z- handleDepth()- Depth_Keller msg: 1730868124.05 epoch sec; seqNo:1007; depth 47.68 2024-11-06T04:42:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868124.07 epoch sec; seqNo:1007; 4061664.56 north; 594263.65 east; 36.70 lat; -121.94 lon; 47.68 depth; utmZone: 10 2024-11-06T04:42:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868123.64 2024-11-06T04:42:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:04Z- waiting for 1730868127.64, time is 1730868124.07 2024-11-06T04:42:04Z- AHRS_M2 msg: 1730868124.45 epoch sec; seqNo:928 2024-11-06T04:42:04Z- AHRS_M2 msg: 0.02 phi; -0.07 theta; 1.86 psi 2024-11-06T04:42:04Z- handleDepth()- Depth_Keller msg: 1730868124.46 epoch sec; seqNo:1008; depth 47.68 2024-11-06T04:42:04Z- handleDVL() - DVL msg epochMillisec: 1730868124.49 2024-11-06T04:42:04Z- handleDvl() - RDI_Pathfinder msg: 1730868124.49 sec; seqNo:336; #beams:4 2024-11-06T04:42:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.94 , 45.94 , 43.73 , 48.59 2024-11-06T04:42:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.14 2024-11-06T04:42:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868124.50 epoch sec; seqNo:1008; 4061664.56 north; 594263.65 east; 36.70 lat; -121.94 lon; 47.68 depth; utmZone: 10 2024-11-06T04:42:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868123.64 2024-11-06T04:42:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:04Z- waiting for 1730868127.64, time is 1730868124.50 2024-11-06T04:42:04Z- AHRS_M2 msg: 1730868124.84 epoch sec; seqNo:929 2024-11-06T04:42:04Z- AHRS_M2 msg: 0.03 phi; -0.07 theta; 1.85 psi 2024-11-06T04:42:04Z- handleDepth()- Depth_Keller msg: 1730868124.85 epoch sec; seqNo:1009; depth 47.69 2024-11-06T04:42:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868124.86 epoch sec; seqNo:1009; 4061664.56 north; 594263.65 east; 36.70 lat; -121.94 lon; 47.69 depth; utmZone: 10 2024-11-06T04:42:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868123.64 2024-11-06T04:42:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:05Z- waiting for 1730868127.64, time is 1730868124.86 2024-11-06T04:42:05Z- handleDepth()- Depth_Keller msg: 1730868125.28 epoch sec; seqNo:1010; depth 47.70 2024-11-06T04:42:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868125.29 epoch sec; seqNo:1010; 4061664.56 north; 594263.65 east; 36.70 lat; -121.94 lon; 47.70 depth; utmZone: 10 2024-11-06T04:42:05Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:42:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868123.64 2024-11-06T04:42:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:05Z- waiting for 1730868127.64, time is 1730868125.29 2024-11-06T04:42:05Z- handleDepth()- Depth_Keller msg: 1730868125.67 epoch sec; seqNo:1011; depth 47.71 2024-11-06T04:42:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868125.68 epoch sec; seqNo:1011; 4061664.57 north; 594264.87 east; 36.70 lat; -121.94 lon; 47.71 depth; utmZone: 10 2024-11-06T04:42:05Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:42:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868123.64 2024-11-06T04:42:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:05Z- waiting for 1730868127.64, time is 1730868125.68 2024-11-06T04:42:06Z- AHRS_M2 msg: 1730868126.05 epoch sec; seqNo:930 2024-11-06T04:42:06Z- AHRS_M2 msg: 0.04 phi; -0.09 theta; 1.82 psi 2024-11-06T04:42:06Z- handleDepth()- Depth_Keller msg: 1730868126.06 epoch sec; seqNo:1012; depth 47.70 2024-11-06T04:42:06Z- handleDVL() - DVL msg epochMillisec: 1730868126.06 2024-11-06T04:42:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:42:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:42:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:42:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:42:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:42:06Z- handleDvl() - RDI_Pathfinder msg: 1730868126.06 sec; seqNo:337; #beams:0 2024-11-06T04:42:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:42:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.14 2024-11-06T04:42:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868126.07 epoch sec; seqNo:1012; 4061664.19 north; 594264.87 east; 36.70 lat; -121.94 lon; 47.70 depth; utmZone: 10 2024-11-06T04:42:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868123.64 2024-11-06T04:42:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:06Z- waiting for 1730868127.64, time is 1730868126.07 2024-11-06T04:42:06Z- AHRS_M2 msg: 1730868126.45 epoch sec; seqNo:931 2024-11-06T04:42:06Z- AHRS_M2 msg: 0.04 phi; -0.09 theta; 1.81 psi 2024-11-06T04:42:06Z- handleDepth()- Depth_Keller msg: 1730868126.46 epoch sec; seqNo:1013; depth 47.72 2024-11-06T04:42:06Z- handleDVL() - DVL msg epochMillisec: 1730868126.48 2024-11-06T04:42:06Z- handleDvl() - RDI_Pathfinder msg: 1730868126.48 sec; seqNo:338; #beams:4 2024-11-06T04:42:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.16 , 45.50 , 43.29 , 49.03 2024-11-06T04:42:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , 0.07 2024-11-06T04:42:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868126.51 epoch sec; seqNo:1013; 4061664.19 north; 594264.87 east; 36.70 lat; -121.94 lon; 47.72 depth; utmZone: 10 2024-11-06T04:42:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868123.64 2024-11-06T04:42:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:06Z- waiting for 1730868127.64, time is 1730868126.51 2024-11-06T04:42:06Z- AHRS_M2 msg: 1730868126.86 epoch sec; seqNo:932 2024-11-06T04:42:06Z- AHRS_M2 msg: 0.04 phi; -0.09 theta; 1.81 psi 2024-11-06T04:42:06Z- handleDepth()- Depth_Keller msg: 1730868126.86 epoch sec; seqNo:1014; depth 47.73 2024-11-06T04:42:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868126.88 epoch sec; seqNo:1014; 4061664.21 north; 594266.09 east; 36.70 lat; -121.94 lon; 47.73 depth; utmZone: 10 2024-11-06T04:42:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868123.64 2024-11-06T04:42:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:07Z- waiting for 1730868127.64, time is 1730868126.88 2024-11-06T04:42:07Z- AHRS_M2 msg: 1730868127.26 epoch sec; seqNo:933 2024-11-06T04:42:07Z- AHRS_M2 msg: 0.04 phi; -0.10 theta; 1.80 psi 2024-11-06T04:42:07Z- handleDepth()- Depth_Keller msg: 1730868127.26 epoch sec; seqNo:1015; depth 47.73 2024-11-06T04:42:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868127.34 epoch sec; seqNo:1015; 4061664.21 north; 594266.09 east; 36.70 lat; -121.94 lon; 47.73 depth; utmZone: 10 2024-11-06T04:42:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868123.64 2024-11-06T04:42:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:07Z- waiting for 1730868127.64, time is 1730868127.34 2024-11-06T04:42:07Z- AHRS_M2 msg: 1730868127.67 epoch sec; seqNo:934 2024-11-06T04:42:07Z- AHRS_M2 msg: 0.05 phi; -0.10 theta; 1.80 psi 2024-11-06T04:42:07Z- handleDepth()- Depth_Keller msg: 1730868127.68 epoch sec; seqNo:1016; depth 47.75 2024-11-06T04:42:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868127.69 epoch sec; seqNo:1016; 4061664.21 north; 594266.09 east; 36.70 lat; -121.94 lon; 47.75 depth; utmZone: 10 2024-11-06T04:42:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868123.64 2024-11-06T04:42:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:07Z- waiting for 1730868127.64, time is 1730868127.69 2024-11-06T04:42:07Z- LcmTrn::updateTrn() >>>> pose time 1730868127.69, meas time 1730868126.48 <<<< 2024-11-06T04:42:07Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868127.69 2024-11-06T04:42:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868126.48 sec, ping # 0; waiting for INS data more recent than 1730868123.64... 2024-11-06T04:42:07Z- TerrainNav::motionUpdate() - currEstimate = (4061664, 594266.09, 47.75) 2024-11-06T04:42:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:42:07Z- TerrainNav:: SubcloudNIS is 0.18, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:42:07Z- TNavPF::Projecting Measurements 2024-11-06T04:42:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:42:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:42:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:42:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:42:07Z- TNavFilter::4 good beams. 2024-11-06T04:42:07Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:42:07Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:42:07Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:42:07Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:42:07Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:42:07Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:42:07Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:42:07Z- TerrainMapOctree: (vehN, vehE ) = (4061664.42, 594264.99). 2024-11-06T04:42:07Z- TerrainMapOctree: Min Distance = 86.88. 2024-11-06T04:42:07Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:42:07Z- Forcing Subcloud Comparison 2024-11-06T04:42:07Z- Weighting particles with subcloud comparison 2024-11-06T04:42:07Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:07Z- meanExpectedMeasDiff: -0.530800 delta_rms_squared: 1.411964 alpha: 0.986033 2024-11-06T04:42:07Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:07Z- meanExpectedMeasDiff: -0.200493 delta_rms_squared: 1.250315 alpha: 0.984256 2024-11-06T04:42:07Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:07Z- meanExpectedMeasDiff: 1.025983 delta_rms_squared: 1.173393 alpha: 0.983241 2024-11-06T04:42:07Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:07Z- meanExpectedMeasDiff: -0.251159 delta_rms_squared: 1.648220 alpha: 0.988011 2024-11-06T04:42:07Z- Subcloud NIS: 0.177750 2024-11-06T04:42:07Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:42:07Z- TNavPF:: sumWeights = 0.604595 2024-11-06T04:42:07Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:42:07Z- TNavPF::First Term of Map Covariance Matrix is 0.44 2024-11-06T04:42:07Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T04:42:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868126.48 sec. 2024-11-06T04:42:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:42:07Z- variances([0], [2], and psi_berg) = 0.159981 0.163523 0.000000 2024-11-06T04:42:07Z- publishEstimatesDecoder() - at 1730868127.69 sec 2024-11-06T04:42:07Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:42:07Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061607.77 594271.26 54.03 2024-11-06T04:42:07Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061608.14 594270.12 54.39 lat/lon: 36.70 -121.94 2024-11-06T04:42:07Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.16 0.73 0.00 2024-11-06T04:42:08Z- AHRS_M2 msg: 1730868128.08 epoch sec; seqNo:935 2024-11-06T04:42:08Z- AHRS_M2 msg: 0.05 phi; -0.10 theta; 1.79 psi 2024-11-06T04:42:08Z- handleDepth()- Depth_Keller msg: 1730868128.10 epoch sec; seqNo:1017; depth 47.76 2024-11-06T04:42:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868128.11 epoch sec; seqNo:1017; 4061663.83 north; 594266.10 east; 36.70 lat; -121.94 lon; 47.76 depth; utmZone: 10 2024-11-06T04:42:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868127.69 2024-11-06T04:42:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:08Z- waiting for 1730868131.69, time is 1730868128.11 2024-11-06T04:42:08Z- AHRS_M2 msg: 1730868128.48 epoch sec; seqNo:936 2024-11-06T04:42:08Z- AHRS_M2 msg: 0.06 phi; -0.11 theta; 1.79 psi 2024-11-06T04:42:08Z- handleDepth()- Depth_Keller msg: 1730868128.49 epoch sec; seqNo:1018; depth 47.76 2024-11-06T04:42:08Z- handleDVL() - DVL msg epochMillisec: 1730868128.52 2024-11-06T04:42:08Z- handleDvl() - RDI_Pathfinder msg: 1730868128.52 sec; seqNo:339; #beams:4 2024-11-06T04:42:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.60 , 45.28 , 42.85 , 49.48 2024-11-06T04:42:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , -0.01 2024-11-06T04:42:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868128.53 epoch sec; seqNo:1018; 4061663.84 north; 594267.32 east; 36.70 lat; -121.94 lon; 47.76 depth; utmZone: 10 2024-11-06T04:42:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868127.69 2024-11-06T04:42:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:08Z- waiting for 1730868131.69, time is 1730868128.53 2024-11-06T04:42:08Z- AHRS_M2 msg: 1730868128.88 epoch sec; seqNo:937 2024-11-06T04:42:08Z- AHRS_M2 msg: 0.07 phi; -0.11 theta; 1.78 psi 2024-11-06T04:42:08Z- handleDepth()- Depth_Keller msg: 1730868128.89 epoch sec; seqNo:1019; depth 47.79 2024-11-06T04:42:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868128.91 epoch sec; seqNo:1019; 4061663.84 north; 594267.32 east; 36.70 lat; -121.94 lon; 47.79 depth; utmZone: 10 2024-11-06T04:42:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868127.69 2024-11-06T04:42:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:09Z- waiting for 1730868131.69, time is 1730868128.91 2024-11-06T04:42:09Z- AHRS_M2 msg: 1730868129.29 epoch sec; seqNo:938 2024-11-06T04:42:09Z- AHRS_M2 msg: 0.07 phi; -0.12 theta; 1.78 psi 2024-11-06T04:42:09Z- handleDepth()- Depth_Keller msg: 1730868129.29 epoch sec; seqNo:1020; depth 47.78 2024-11-06T04:42:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868129.31 epoch sec; seqNo:1020; 4061663.84 north; 594267.32 east; 36.70 lat; -121.94 lon; 47.78 depth; utmZone: 10 2024-11-06T04:42:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868127.69 2024-11-06T04:42:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:09Z- waiting for 1730868131.69, time is 1730868129.31 2024-11-06T04:42:09Z- AHRS_M2 msg: 1730868129.69 epoch sec; seqNo:939 2024-11-06T04:42:09Z- AHRS_M2 msg: 0.07 phi; -0.12 theta; 1.77 psi 2024-11-06T04:42:09Z- handleDepth()- Depth_Keller msg: 1730868129.70 epoch sec; seqNo:1021; depth 47.80 2024-11-06T04:42:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868129.71 epoch sec; seqNo:1021; 4061663.85 north; 594268.54 east; 36.70 lat; -121.94 lon; 47.80 depth; utmZone: 10 2024-11-06T04:42:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868127.69 2024-11-06T04:42:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:09Z- waiting for 1730868131.69, time is 1730868129.71 2024-11-06T04:42:10Z- AHRS_M2 msg: 1730868130.10 epoch sec; seqNo:940 2024-11-06T04:42:10Z- AHRS_M2 msg: 0.07 phi; -0.12 theta; 1.77 psi 2024-11-06T04:42:10Z- handleDepth()- Depth_Keller msg: 1730868130.11 epoch sec; seqNo:1022; depth 47.81 2024-11-06T04:42:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868130.12 epoch sec; seqNo:1022; 4061663.48 north; 594268.54 east; 36.70 lat; -121.94 lon; 47.81 depth; utmZone: 10 2024-11-06T04:42:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868127.69 2024-11-06T04:42:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:10Z- waiting for 1730868131.69, time is 1730868130.12 2024-11-06T04:42:10Z- handleDepth()- Depth_Keller msg: 1730868130.52 epoch sec; seqNo:1023; depth 47.82 2024-11-06T04:42:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868130.55 epoch sec; seqNo:1023; 4061663.48 north; 594268.54 east; 36.70 lat; -121.94 lon; 47.82 depth; utmZone: 10 2024-11-06T04:42:10Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:42:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868127.69 2024-11-06T04:42:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:10Z- waiting for 1730868131.69, time is 1730868130.55 2024-11-06T04:42:10Z- AHRS_M2 msg: 1730868130.90 epoch sec; seqNo:941 2024-11-06T04:42:10Z- AHRS_M2 msg: 0.08 phi; -0.13 theta; 1.75 psi 2024-11-06T04:42:10Z- handleDepth()- Depth_Keller msg: 1730868130.91 epoch sec; seqNo:1024; depth 47.84 2024-11-06T04:42:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868130.92 epoch sec; seqNo:1024; 4061663.48 north; 594268.54 east; 36.70 lat; -121.94 lon; 47.84 depth; utmZone: 10 2024-11-06T04:42:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868127.69 2024-11-06T04:42:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:11Z- waiting for 1730868131.69, time is 1730868130.92 2024-11-06T04:42:11Z- AHRS_M2 msg: 1730868131.31 epoch sec; seqNo:942 2024-11-06T04:42:11Z- AHRS_M2 msg: 0.08 phi; -0.13 theta; 1.74 psi 2024-11-06T04:42:11Z- handleDepth()- Depth_Keller msg: 1730868131.32 epoch sec; seqNo:1025; depth 47.85 2024-11-06T04:42:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868131.33 epoch sec; seqNo:1025; 4061663.49 north; 594269.76 east; 36.70 lat; -121.94 lon; 47.85 depth; utmZone: 10 2024-11-06T04:42:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868127.69 2024-11-06T04:42:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:11Z- waiting for 1730868131.69, time is 1730868131.33 2024-11-06T04:42:11Z- AHRS_M2 msg: 1730868131.71 epoch sec; seqNo:943 2024-11-06T04:42:11Z- AHRS_M2 msg: 0.08 phi; -0.14 theta; 1.74 psi 2024-11-06T04:42:11Z- handleDepth()- Depth_Keller msg: 1730868131.72 epoch sec; seqNo:1026; depth 47.86 2024-11-06T04:42:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868131.74 epoch sec; seqNo:1026; 4061663.49 north; 594269.76 east; 36.70 lat; -121.94 lon; 47.86 depth; utmZone: 10 2024-11-06T04:42:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868127.69 2024-11-06T04:42:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:11Z- waiting for 1730868131.69, time is 1730868131.74 2024-11-06T04:42:11Z- LcmTrn::updateTrn() >>>> pose time 1730868131.74, meas time 1730868128.52 <<<< 2024-11-06T04:42:11Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868131.74 2024-11-06T04:42:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868128.52 sec, ping # 0; waiting for INS data more recent than 1730868127.69... 2024-11-06T04:42:11Z- TerrainNav::motionUpdate() - currEstimate = (4061663, 594269.76, 47.86) 2024-11-06T04:42:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:42:11Z- TerrainNav:: SubcloudNIS is 0.18, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:42:11Z- TNavPF::Projecting Measurements 2024-11-06T04:42:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:42:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:42:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:42:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:42:11Z- TNavFilter::4 good beams. 2024-11-06T04:42:11Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:42:11Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:42:11Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:42:11Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:42:11Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:42:11Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:42:11Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:42:11Z- TerrainMapOctree: (vehN, vehE ) = (4061664.06, 594266.84). 2024-11-06T04:42:11Z- TerrainMapOctree: Min Distance = 87.57. 2024-11-06T04:42:11Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:42:11Z- Forcing Subcloud Comparison 2024-11-06T04:42:11Z- Weighting particles with subcloud comparison 2024-11-06T04:42:11Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:11Z- meanExpectedMeasDiff: -0.533053 delta_rms_squared: 1.472835 alpha: 0.986603 2024-11-06T04:42:11Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:11Z- meanExpectedMeasDiff: -0.020770 delta_rms_squared: 1.438563 alpha: 0.986288 2024-11-06T04:42:12Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:12Z- meanExpectedMeasDiff: 0.952247 delta_rms_squared: 1.168338 alpha: 0.983170 2024-11-06T04:42:12Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:12Z- meanExpectedMeasDiff: -0.393131 delta_rms_squared: 1.815299 alpha: 0.989103 2024-11-06T04:42:12Z- Subcloud NIS: 0.181175 2024-11-06T04:42:12Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:42:12Z- TNavPF:: sumWeights = 0.593233 2024-11-06T04:42:12Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:42:12Z- TNavPF::First Term of Map Covariance Matrix is 0.45 2024-11-06T04:42:12Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T04:42:12Z- TerrainNav::Resampling particle filter... 2024-11-06T04:42:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868128.52 sec. 2024-11-06T04:42:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:42:12Z- variances([0], [2], and psi_berg) = 0.159037 0.166565 0.000000 2024-11-06T04:42:12Z- publishEstimatesDecoder() - at 1730868131.74 sec 2024-11-06T04:42:12Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:42:12Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061607.73 594273.93 53.62 2024-11-06T04:42:12Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061607.42 594273.78 54.51 lat/lon: 36.70 -121.94 2024-11-06T04:42:12Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.17 0.72 0.00 2024-11-06T04:42:12Z- AHRS_M2 msg: 1730868132.12 epoch sec; seqNo:944 2024-11-06T04:42:12Z- AHRS_M2 msg: 0.08 phi; -0.14 theta; 1.73 psi 2024-11-06T04:42:12Z- handleDepth()- Depth_Keller msg: 1730868132.12 epoch sec; seqNo:1027; depth 47.88 2024-11-06T04:42:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868132.13 epoch sec; seqNo:1027; 4061663.49 north; 594269.76 east; 36.70 lat; -121.94 lon; 47.88 depth; utmZone: 10 2024-11-06T04:42:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868131.74 2024-11-06T04:42:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:12Z- waiting for 1730868135.74, time is 1730868132.13 2024-11-06T04:42:12Z- AHRS_M2 msg: 1730868132.52 epoch sec; seqNo:945 2024-11-06T04:42:12Z- AHRS_M2 msg: 0.08 phi; -0.14 theta; 1.72 psi 2024-11-06T04:42:12Z- handleDepth()- Depth_Keller msg: 1730868132.53 epoch sec; seqNo:1028; depth 47.90 2024-11-06T04:42:12Z- handleDVL() - DVL msg epochMillisec: 1730868132.55 2024-11-06T04:42:12Z- handleDvl() - RDI_Pathfinder msg: 1730868132.55 sec; seqNo:340; #beams:4 2024-11-06T04:42:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.05 , 44.40 , 41.97 , 50.36 2024-11-06T04:42:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.05 , 0.02 2024-11-06T04:42:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868132.56 epoch sec; seqNo:1028; 4061663.50 north; 594270.98 east; 36.70 lat; -121.94 lon; 47.90 depth; utmZone: 10 2024-11-06T04:42:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868131.74 2024-11-06T04:42:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:12Z- waiting for 1730868135.74, time is 1730868132.56 2024-11-06T04:42:12Z- AHRS_M2 msg: 1730868132.92 epoch sec; seqNo:946 2024-11-06T04:42:12Z- AHRS_M2 msg: 0.08 phi; -0.14 theta; 1.71 psi 2024-11-06T04:42:12Z- handleDepth()- Depth_Keller msg: 1730868132.93 epoch sec; seqNo:1029; depth 47.91 2024-11-06T04:42:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868132.95 epoch sec; seqNo:1029; 4061663.50 north; 594270.98 east; 36.70 lat; -121.94 lon; 47.91 depth; utmZone: 10 2024-11-06T04:42:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868131.74 2024-11-06T04:42:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:13Z- waiting for 1730868135.74, time is 1730868132.95 2024-11-06T04:42:13Z- AHRS_M2 msg: 1730868133.33 epoch sec; seqNo:947 2024-11-06T04:42:13Z- AHRS_M2 msg: 0.08 phi; -0.15 theta; 1.71 psi 2024-11-06T04:42:13Z- handleDepth()- Depth_Keller msg: 1730868133.33 epoch sec; seqNo:1030; depth 47.94 2024-11-06T04:42:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868133.35 epoch sec; seqNo:1030; 4061663.50 north; 594270.98 east; 36.70 lat; -121.94 lon; 47.94 depth; utmZone: 10 2024-11-06T04:42:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868131.74 2024-11-06T04:42:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:13Z- waiting for 1730868135.74, time is 1730868133.35 2024-11-06T04:42:13Z- AHRS_M2 msg: 1730868133.73 epoch sec; seqNo:948 2024-11-06T04:42:13Z- AHRS_M2 msg: 0.08 phi; -0.15 theta; 1.70 psi 2024-11-06T04:42:13Z- handleDepth()- Depth_Keller msg: 1730868133.74 epoch sec; seqNo:1031; depth 47.95 2024-11-06T04:42:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868133.75 epoch sec; seqNo:1031; 4061663.50 north; 594270.98 east; 36.70 lat; -121.94 lon; 47.95 depth; utmZone: 10 2024-11-06T04:42:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868131.74 2024-11-06T04:42:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:13Z- waiting for 1730868135.74, time is 1730868133.75 2024-11-06T04:42:14Z- AHRS_M2 msg: 1730868134.13 epoch sec; seqNo:949 2024-11-06T04:42:14Z- AHRS_M2 msg: 0.08 phi; -0.15 theta; 1.69 psi 2024-11-06T04:42:14Z- handleDepth()- Depth_Keller msg: 1730868134.14 epoch sec; seqNo:1032; depth 47.98 2024-11-06T04:42:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868134.15 epoch sec; seqNo:1032; 4061663.52 north; 594272.20 east; 36.70 lat; -121.94 lon; 47.98 depth; utmZone: 10 2024-11-06T04:42:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868131.74 2024-11-06T04:42:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:14Z- waiting for 1730868135.74, time is 1730868134.15 2024-11-06T04:42:14Z- AHRS_M2 msg: 1730868134.54 epoch sec; seqNo:950 2024-11-06T04:42:14Z- AHRS_M2 msg: 0.07 phi; -0.15 theta; 1.68 psi 2024-11-06T04:42:14Z- handleDepth()- Depth_Keller msg: 1730868134.55 epoch sec; seqNo:1033; depth 48.01 2024-11-06T04:42:14Z- handleDVL() - DVL msg epochMillisec: 1730868134.59 2024-11-06T04:42:14Z- handleDvl() - RDI_Pathfinder msg: 1730868134.59 sec; seqNo:341; #beams:4 2024-11-06T04:42:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.83 , 44.40 , 41.53 , 51.02 2024-11-06T04:42:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.06 , 0.08 2024-11-06T04:42:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868134.60 epoch sec; seqNo:1033; 4061663.52 north; 594272.20 east; 36.70 lat; -121.94 lon; 48.01 depth; utmZone: 10 2024-11-06T04:42:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868131.74 2024-11-06T04:42:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:14Z- waiting for 1730868135.74, time is 1730868134.60 2024-11-06T04:42:14Z- AHRS_M2 msg: 1730868134.94 epoch sec; seqNo:951 2024-11-06T04:42:14Z- AHRS_M2 msg: 0.07 phi; -0.16 theta; 1.68 psi 2024-11-06T04:42:14Z- handleDepth()- Depth_Keller msg: 1730868134.95 epoch sec; seqNo:1034; depth 48.05 2024-11-06T04:42:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868134.97 epoch sec; seqNo:1034; 4061663.14 north; 594272.21 east; 36.70 lat; -121.94 lon; 48.05 depth; utmZone: 10 2024-11-06T04:42:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868131.74 2024-11-06T04:42:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:15Z- waiting for 1730868135.74, time is 1730868134.97 2024-11-06T04:42:15Z- AHRS_M2 msg: 1730868135.35 epoch sec; seqNo:952 2024-11-06T04:42:15Z- AHRS_M2 msg: 0.07 phi; -0.16 theta; 1.67 psi 2024-11-06T04:42:15Z- handleDepth()- Depth_Keller msg: 1730868135.36 epoch sec; seqNo:1035; depth 48.07 2024-11-06T04:42:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868135.38 epoch sec; seqNo:1035; 4061663.14 north; 594272.21 east; 36.70 lat; -121.94 lon; 48.07 depth; utmZone: 10 2024-11-06T04:42:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868131.74 2024-11-06T04:42:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:15Z- waiting for 1730868135.74, time is 1730868135.38 2024-11-06T04:42:15Z- AHRS_M2 msg: 1730868135.75 epoch sec; seqNo:953 2024-11-06T04:42:15Z- AHRS_M2 msg: 0.07 phi; -0.16 theta; 1.66 psi 2024-11-06T04:42:15Z- handleDepth()- Depth_Keller msg: 1730868135.76 epoch sec; seqNo:1036; depth 48.08 2024-11-06T04:42:15Z- handleDVL() - DVL msg epochMillisec: 1730868135.76 2024-11-06T04:42:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:42:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:42:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:42:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:42:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:42:15Z- handleDvl() - RDI_Pathfinder msg: 1730868135.76 sec; seqNo:342; #beams:0 2024-11-06T04:42:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:42:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.06 , 0.08 2024-11-06T04:42:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868135.78 epoch sec; seqNo:1036; 4061663.15 north; 594273.43 east; 36.70 lat; -121.94 lon; 48.08 depth; utmZone: 10 2024-11-06T04:42:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868131.74 2024-11-06T04:42:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:15Z- waiting for 1730868135.74, time is 1730868135.78 2024-11-06T04:42:15Z- LcmTrn::updateTrn() >>>> pose time 1730868135.78, meas time 1730868135.76 <<<< 2024-11-06T04:42:15Z- LcmTrn::updateTrn() equating pose 1730868135.76 and meas times 1730868135.76 2024-11-06T04:42:15Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868135.76 2024-11-06T04:42:15Z- TerrainNav::motionUpdate() - currEstimate = (4061663, 594273.43, 48.08) 2024-11-06T04:42:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:42:15Z- TerrainNav:: SubcloudNIS is 0.18, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:42:15Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:42:15Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:42:15Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:42:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:42:15Z- variances([0], [2], and psi_berg) = 0.161265 0.168606 0.000000 2024-11-06T04:42:15Z- publishEstimatesDecoder() - at 1730868135.76 sec 2024-11-06T04:42:15Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:42:15Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061607.40 594277.68 54.70 2024-11-06T04:42:15Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061607.08 594277.44 54.73 lat/lon: 36.70 -121.94 2024-11-06T04:42:15Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.17 0.98 0.00 2024-11-06T04:42:16Z- AHRS_M2 msg: 1730868136.15 epoch sec; seqNo:954 2024-11-06T04:42:16Z- AHRS_M2 msg: 0.07 phi; -0.16 theta; 1.66 psi 2024-11-06T04:42:16Z- handleDepth()- Depth_Keller msg: 1730868136.16 epoch sec; seqNo:1037; depth 48.12 2024-11-06T04:42:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868136.18 epoch sec; seqNo:1037; 4061663.15 north; 594273.43 east; 36.70 lat; -121.94 lon; 48.12 depth; utmZone: 10 2024-11-06T04:42:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868135.76 2024-11-06T04:42:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:16Z- waiting for 1730868139.76, time is 1730868136.18 2024-11-06T04:42:16Z- AHRS_M2 msg: 1730868136.56 epoch sec; seqNo:955 2024-11-06T04:42:16Z- AHRS_M2 msg: 0.07 phi; -0.17 theta; 1.65 psi 2024-11-06T04:42:16Z- handleDepth()- Depth_Keller msg: 1730868136.56 epoch sec; seqNo:1038; depth 48.13 2024-11-06T04:42:16Z- handleDVL() - DVL msg epochMillisec: 1730868136.58 2024-11-06T04:42:16Z- handleDvl() - RDI_Pathfinder msg: 1730868136.58 sec; seqNo:343; #beams:4 2024-11-06T04:42:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.83 , 44.40 , 41.30 , 51.24 2024-11-06T04:42:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , 0.08 2024-11-06T04:42:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868136.60 epoch sec; seqNo:1038; 4061663.15 north; 594273.43 east; 36.70 lat; -121.94 lon; 48.13 depth; utmZone: 10 2024-11-06T04:42:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868135.76 2024-11-06T04:42:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:16Z- waiting for 1730868139.76, time is 1730868136.60 2024-11-06T04:42:16Z- AHRS_M2 msg: 1730868136.96 epoch sec; seqNo:956 2024-11-06T04:42:16Z- AHRS_M2 msg: 0.07 phi; -0.17 theta; 1.65 psi 2024-11-06T04:42:16Z- handleDepth()- Depth_Keller msg: 1730868136.98 epoch sec; seqNo:1039; depth 48.19 2024-11-06T04:42:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868137.00 epoch sec; seqNo:1039; 4061663.15 north; 594273.43 east; 36.70 lat; -121.94 lon; 48.19 depth; utmZone: 10 2024-11-06T04:42:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868135.76 2024-11-06T04:42:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:17Z- waiting for 1730868139.76, time is 1730868137.00 2024-11-06T04:42:17Z- AHRS_M2 msg: 1730868137.37 epoch sec; seqNo:957 2024-11-06T04:42:17Z- AHRS_M2 msg: 0.07 phi; -0.17 theta; 1.64 psi 2024-11-06T04:42:17Z- handleDepth()- Depth_Keller msg: 1730868137.38 epoch sec; seqNo:1040; depth 48.22 2024-11-06T04:42:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868137.39 epoch sec; seqNo:1040; 4061663.16 north; 594274.65 east; 36.70 lat; -121.94 lon; 48.22 depth; utmZone: 10 2024-11-06T04:42:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868135.76 2024-11-06T04:42:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:17Z- waiting for 1730868139.76, time is 1730868137.39 2024-11-06T04:42:17Z- AHRS_M2 msg: 1730868137.77 epoch sec; seqNo:958 2024-11-06T04:42:17Z- AHRS_M2 msg: 0.08 phi; -0.17 theta; 1.64 psi 2024-11-06T04:42:17Z- handleDepth()- Depth_Keller msg: 1730868137.78 epoch sec; seqNo:1041; depth 48.27 2024-11-06T04:42:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868137.79 epoch sec; seqNo:1041; 4061663.16 north; 594274.65 east; 36.70 lat; -121.94 lon; 48.27 depth; utmZone: 10 2024-11-06T04:42:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868135.76 2024-11-06T04:42:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:17Z- waiting for 1730868139.76, time is 1730868137.79 2024-11-06T04:42:18Z- handleDepth()- Depth_Keller msg: 1730868138.20 epoch sec; seqNo:1042; depth 48.29 2024-11-06T04:42:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868138.21 epoch sec; seqNo:1042; 4061663.16 north; 594274.65 east; 36.70 lat; -121.94 lon; 48.29 depth; utmZone: 10 2024-11-06T04:42:18Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:42:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868135.76 2024-11-06T04:42:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:18Z- waiting for 1730868139.76, time is 1730868138.21 2024-11-06T04:42:18Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:42:18Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:42:18Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:42:18Z- AHRS_M2 msg: 1730868138.57 epoch sec; seqNo:959 2024-11-06T04:42:18Z- AHRS_M2 msg: 0.08 phi; -0.17 theta; 1.64 psi 2024-11-06T04:42:18Z- handleDepth()- Depth_Keller msg: 1730868138.60 epoch sec; seqNo:1043; depth 48.33 2024-11-06T04:42:18Z- handleDVL() - DVL msg epochMillisec: 1730868138.62 2024-11-06T04:42:18Z- handleDvl() - RDI_Pathfinder msg: 1730868138.62 sec; seqNo:344; #beams:4 2024-11-06T04:42:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.05 , 44.17 , 41.08 , 51.46 2024-11-06T04:42:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , 0.04 2024-11-06T04:42:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868138.63 epoch sec; seqNo:1043; 4061663.18 north; 594275.87 east; 36.70 lat; -121.94 lon; 48.33 depth; utmZone: 10 2024-11-06T04:42:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868135.76 2024-11-06T04:42:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:18Z- waiting for 1730868139.76, time is 1730868138.63 2024-11-06T04:42:18Z- AHRS_M2 msg: 1730868138.98 epoch sec; seqNo:960 2024-11-06T04:42:19Z- AHRS_M2 msg: 0.09 phi; -0.18 theta; 1.63 psi 2024-11-06T04:42:19Z- handleDepth()- Depth_Keller msg: 1730868138.99 epoch sec; seqNo:1044; depth 48.37 2024-11-06T04:42:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868139.02 epoch sec; seqNo:1044; 4061663.18 north; 594275.87 east; 36.70 lat; -121.94 lon; 48.37 depth; utmZone: 10 2024-11-06T04:42:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868135.76 2024-11-06T04:42:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:19Z- waiting for 1730868139.76, time is 1730868139.02 2024-11-06T04:42:19Z- AHRS_M2 msg: 1730868139.39 epoch sec; seqNo:961 2024-11-06T04:42:19Z- AHRS_M2 msg: 0.09 phi; -0.18 theta; 1.63 psi 2024-11-06T04:42:19Z- handleDepth()- Depth_Keller msg: 1730868139.40 epoch sec; seqNo:1045; depth 48.41 2024-11-06T04:42:19Z- handleDVL() - DVL msg epochMillisec: 1730868139.41 2024-11-06T04:42:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:42:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:42:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:42:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:42:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:42:19Z- handleDvl() - RDI_Pathfinder msg: 1730868139.41 sec; seqNo:345; #beams:0 2024-11-06T04:42:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:42:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , 0.04 2024-11-06T04:42:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868139.41 epoch sec; seqNo:1045; 4061663.18 north; 594275.87 east; 36.70 lat; -121.94 lon; 48.41 depth; utmZone: 10 2024-11-06T04:42:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868135.76 2024-11-06T04:42:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:19Z- waiting for 1730868139.76, time is 1730868139.41 2024-11-06T04:42:19Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:42:19Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:42:19Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:42:19Z- AHRS_M2 msg: 1730868139.78 epoch sec; seqNo:962 2024-11-06T04:42:19Z- AHRS_M2 msg: 0.09 phi; -0.18 theta; 1.63 psi 2024-11-06T04:42:19Z- handleDepth()- Depth_Keller msg: 1730868139.82 epoch sec; seqNo:1046; depth 48.45 2024-11-06T04:42:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868139.83 epoch sec; seqNo:1046; 4061663.18 north; 594275.87 east; 36.70 lat; -121.94 lon; 48.45 depth; utmZone: 10 2024-11-06T04:42:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868135.76 2024-11-06T04:42:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:19Z- waiting for 1730868139.76, time is 1730868139.83 2024-11-06T04:42:19Z- LcmTrn::updateTrn() >>>> pose time 1730868139.83, meas time 1730868139.41 <<<< 2024-11-06T04:42:19Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868139.83 2024-11-06T04:42:19Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:42:19Z- TerrainNav::motionUpdate() - currEstimate = (4061663, 594275.87, 48.45) 2024-11-06T04:42:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:42:19Z- TerrainNav:: SubcloudNIS is 0.18, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:42:19Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:42:19Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:42:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:42:20Z- variances([0], [2], and psi_berg) = 0.162066 0.169628 0.000000 2024-11-06T04:42:20Z- publishEstimatesDecoder() - at 1730868139.83 sec 2024-11-06T04:42:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:42:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061607.42 594280.15 55.24 2024-11-06T04:42:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061607.11 594279.88 55.11 lat/lon: 36.70 -121.94 2024-11-06T04:42:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.17 1.23 0.00 2024-11-06T04:42:20Z- AHRS_M2 msg: 1730868140.20 epoch sec; seqNo:963 2024-11-06T04:42:20Z- AHRS_M2 msg: 0.10 phi; -0.18 theta; 1.62 psi 2024-11-06T04:42:20Z- handleDepth()- Depth_Keller msg: 1730868140.21 epoch sec; seqNo:1047; depth 48.49 2024-11-06T04:42:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868140.22 epoch sec; seqNo:1047; 4061663.19 north; 594277.09 east; 36.70 lat; -121.94 lon; 48.49 depth; utmZone: 10 2024-11-06T04:42:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868139.83 2024-11-06T04:42:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:20Z- waiting for 1730868143.83, time is 1730868140.22 2024-11-06T04:42:20Z- AHRS_M2 msg: 1730868140.60 epoch sec; seqNo:964 2024-11-06T04:42:20Z- AHRS_M2 msg: 0.10 phi; -0.18 theta; 1.62 psi 2024-11-06T04:42:20Z- handleDepth()- Depth_Keller msg: 1730868140.61 epoch sec; seqNo:1048; depth 48.54 2024-11-06T04:42:20Z- handleDVL() - DVL msg epochMillisec: 1730868140.62 2024-11-06T04:42:20Z- handleDvl() - RDI_Pathfinder msg: 1730868140.62 sec; seqNo:346; #beams:4 2024-11-06T04:42:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.27 , 43.52 , 40.65 , 51.69 2024-11-06T04:42:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , 0.01 2024-11-06T04:42:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868140.65 epoch sec; seqNo:1048; 4061663.19 north; 594277.09 east; 36.70 lat; -121.94 lon; 48.54 depth; utmZone: 10 2024-11-06T04:42:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868139.83 2024-11-06T04:42:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:20Z- waiting for 1730868143.83, time is 1730868140.65 2024-11-06T04:42:21Z- AHRS_M2 msg: 1730868141.00 epoch sec; seqNo:965 2024-11-06T04:42:21Z- AHRS_M2 msg: 0.10 phi; -0.18 theta; 1.61 psi 2024-11-06T04:42:21Z- handleDepth()- Depth_Keller msg: 1730868141.02 epoch sec; seqNo:1049; depth 48.57 2024-11-06T04:42:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868141.03 epoch sec; seqNo:1049; 4061663.19 north; 594277.09 east; 36.70 lat; -121.94 lon; 48.57 depth; utmZone: 10 2024-11-06T04:42:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868139.83 2024-11-06T04:42:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:21Z- waiting for 1730868143.83, time is 1730868141.03 2024-11-06T04:42:21Z- AHRS_M2 msg: 1730868141.41 epoch sec; seqNo:966 2024-11-06T04:42:21Z- AHRS_M2 msg: 0.10 phi; -0.18 theta; 1.61 psi 2024-11-06T04:42:21Z- handleDepth()- Depth_Keller msg: 1730868141.41 epoch sec; seqNo:1050; depth 48.60 2024-11-06T04:42:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868141.42 epoch sec; seqNo:1050; 4061663.19 north; 594277.09 east; 36.70 lat; -121.94 lon; 48.60 depth; utmZone: 10 2024-11-06T04:42:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868139.83 2024-11-06T04:42:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:21Z- waiting for 1730868143.83, time is 1730868141.42 2024-11-06T04:42:21Z- AHRS_M2 msg: 1730868141.81 epoch sec; seqNo:967 2024-11-06T04:42:21Z- AHRS_M2 msg: 0.10 phi; -0.18 theta; 1.60 psi 2024-11-06T04:42:21Z- handleDepth()- Depth_Keller msg: 1730868141.82 epoch sec; seqNo:1051; depth 48.64 2024-11-06T04:42:21Z- handleDVL() - DVL msg epochMillisec: 1730868141.83 2024-11-06T04:42:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:42:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:42:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:42:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:42:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:42:21Z- handleDvl() - RDI_Pathfinder msg: 1730868141.83 sec; seqNo:347; #beams:0 2024-11-06T04:42:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:42:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , 0.01 2024-11-06T04:42:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868141.85 epoch sec; seqNo:1051; 4061663.20 north; 594278.31 east; 36.70 lat; -121.94 lon; 48.64 depth; utmZone: 10 2024-11-06T04:42:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868139.83 2024-11-06T04:42:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:21Z- waiting for 1730868143.83, time is 1730868141.85 2024-11-06T04:42:22Z- AHRS_M2 msg: 1730868142.22 epoch sec; seqNo:968 2024-11-06T04:42:22Z- AHRS_M2 msg: 0.10 phi; -0.18 theta; 1.60 psi 2024-11-06T04:42:22Z- handleDepth()- Depth_Keller msg: 1730868142.22 epoch sec; seqNo:1052; depth 48.71 2024-11-06T04:42:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868142.23 epoch sec; seqNo:1052; 4061663.20 north; 594278.31 east; 36.70 lat; -121.94 lon; 48.71 depth; utmZone: 10 2024-11-06T04:42:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868139.83 2024-11-06T04:42:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:22Z- waiting for 1730868143.83, time is 1730868142.23 2024-11-06T04:42:22Z- AHRS_M2 msg: 1730868142.63 epoch sec; seqNo:969 2024-11-06T04:42:22Z- AHRS_M2 msg: 0.10 phi; -0.18 theta; 1.59 psi 2024-11-06T04:42:22Z- handleDepth()- Depth_Keller msg: 1730868142.63 epoch sec; seqNo:1053; depth 48.73 2024-11-06T04:42:22Z- handleDVL() - DVL msg epochMillisec: 1730868142.65 2024-11-06T04:42:22Z- handleDvl() - RDI_Pathfinder msg: 1730868142.65 sec; seqNo:348; #beams:4 2024-11-06T04:42:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 43.08 , 40.42 , 51.91 2024-11-06T04:42:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.07 , 0.01 2024-11-06T04:42:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868142.66 epoch sec; seqNo:1053; 4061663.20 north; 594278.31 east; 36.70 lat; -121.94 lon; 48.73 depth; utmZone: 10 2024-11-06T04:42:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868139.83 2024-11-06T04:42:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:22Z- waiting for 1730868143.83, time is 1730868142.66 2024-11-06T04:42:23Z- AHRS_M2 msg: 1730868143.03 epoch sec; seqNo:970 2024-11-06T04:42:23Z- AHRS_M2 msg: 0.10 phi; -0.18 theta; 1.59 psi 2024-11-06T04:42:23Z- handleDepth()- Depth_Keller msg: 1730868143.04 epoch sec; seqNo:1054; depth 48.79 2024-11-06T04:42:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868143.07 epoch sec; seqNo:1054; 4061663.60 north; 594279.53 east; 36.70 lat; -121.94 lon; 48.79 depth; utmZone: 10 2024-11-06T04:42:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868139.83 2024-11-06T04:42:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:23Z- waiting for 1730868143.83, time is 1730868143.07 2024-11-06T04:42:23Z- AHRS_M2 msg: 1730868143.43 epoch sec; seqNo:971 2024-11-06T04:42:23Z- AHRS_M2 msg: 0.10 phi; -0.19 theta; 1.58 psi 2024-11-06T04:42:23Z- handleDepth()- Depth_Keller msg: 1730868143.43 epoch sec; seqNo:1055; depth 48.81 2024-11-06T04:42:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868143.44 epoch sec; seqNo:1055; 4061663.60 north; 594279.53 east; 36.70 lat; -121.94 lon; 48.81 depth; utmZone: 10 2024-11-06T04:42:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868139.83 2024-11-06T04:42:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:23Z- waiting for 1730868143.83, time is 1730868143.44 2024-11-06T04:42:23Z- AHRS_M2 msg: 1730868143.84 epoch sec; seqNo:972 2024-11-06T04:42:23Z- AHRS_M2 msg: 0.10 phi; -0.19 theta; 1.58 psi 2024-11-06T04:42:23Z- handleDepth()- Depth_Keller msg: 1730868143.84 epoch sec; seqNo:1056; depth 48.85 2024-11-06T04:42:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868143.85 epoch sec; seqNo:1056; 4061663.60 north; 594279.53 east; 36.70 lat; -121.94 lon; 48.85 depth; utmZone: 10 2024-11-06T04:42:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868139.83 2024-11-06T04:42:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:23Z- waiting for 1730868143.83, time is 1730868143.85 2024-11-06T04:42:24Z- LcmTrn::updateTrn() >>>> pose time 1730868143.85, meas time 1730868142.65 <<<< 2024-11-06T04:42:24Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868143.85 2024-11-06T04:42:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868142.65 sec, ping # 0; waiting for INS data more recent than 1730868139.83... 2024-11-06T04:42:24Z- TerrainNav::motionUpdate() - currEstimate = (4061664, 594279.53, 48.85) 2024-11-06T04:42:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:42:24Z- TerrainNav:: SubcloudNIS is 0.18, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:42:24Z- TNavPF::Projecting Measurements 2024-11-06T04:42:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:42:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:42:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:42:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:42:24Z- TNavFilter::4 good beams. 2024-11-06T04:42:24Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:42:24Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:42:24Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:42:24Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:42:24Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:42:24Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:42:24Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:42:24Z- TerrainMapOctree: (vehN, vehE ) = (4061663.47, 594278.43). 2024-11-06T04:42:24Z- TerrainMapOctree: Min Distance = 91.08. 2024-11-06T04:42:24Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:42:24Z- Forcing Subcloud Comparison 2024-11-06T04:42:24Z- Weighting particles with subcloud comparison 2024-11-06T04:42:24Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:24Z- meanExpectedMeasDiff: 0.993107 delta_rms_squared: 2.340464 alpha: 0.991527 2024-11-06T04:42:24Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:24Z- meanExpectedMeasDiff: 0.346526 delta_rms_squared: 1.845464 alpha: 0.989279 2024-11-06T04:42:24Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:24Z- meanExpectedMeasDiff: 0.711952 delta_rms_squared: 1.657334 alpha: 0.988076 2024-11-06T04:42:24Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:24Z- meanExpectedMeasDiff: -0.066168 delta_rms_squared: 2.597666 alpha: 0.992360 2024-11-06T04:42:24Z- Subcloud NIS: 0.187778 2024-11-06T04:42:24Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:42:24Z- TNavPF:: sumWeights = 0.557054 2024-11-06T04:42:24Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:42:24Z- TNavPF::First Term of Map Covariance Matrix is 0.55 2024-11-06T04:42:24Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.02 2024-11-06T04:42:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868142.65 sec. 2024-11-06T04:42:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:42:24Z- variances([0], [2], and psi_berg) = 0.162254 0.172013 0.000000 2024-11-06T04:42:24Z- publishEstimatesDecoder() - at 1730868143.85 sec 2024-11-06T04:42:24Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:42:24Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061607.25 594284.23 53.81 2024-11-06T04:42:24Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061607.53 594283.54 55.24 lat/lon: 36.70 -121.94 2024-11-06T04:42:24Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.17 0.82 0.00 2024-11-06T04:42:24Z- AHRS_M2 msg: 1730868144.25 epoch sec; seqNo:973 2024-11-06T04:42:24Z- AHRS_M2 msg: 0.11 phi; -0.19 theta; 1.57 psi 2024-11-06T04:42:24Z- handleDepth()- Depth_Keller msg: 1730868144.26 epoch sec; seqNo:1057; depth 48.91 2024-11-06T04:42:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868144.26 epoch sec; seqNo:1057; 4061663.60 north; 594279.53 east; 36.70 lat; -121.94 lon; 48.91 depth; utmZone: 10 2024-11-06T04:42:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868143.85 2024-11-06T04:42:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:24Z- waiting for 1730868147.85, time is 1730868144.26 2024-11-06T04:42:24Z- handleDepth()- Depth_Keller msg: 1730868144.66 epoch sec; seqNo:1058; depth 48.94 2024-11-06T04:42:24Z- handleDVL() - DVL msg epochMillisec: 1730868144.68 2024-11-06T04:42:24Z- handleDvl() - RDI_Pathfinder msg: 1730868144.68 sec; seqNo:349; #beams:4 2024-11-06T04:42:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.05 , 42.85 , 40.20 , 51.46 2024-11-06T04:42:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , 0.04 2024-11-06T04:42:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868144.69 epoch sec; seqNo:1058; 4061663.61 north; 594280.75 east; 36.70 lat; -121.94 lon; 48.94 depth; utmZone: 10 2024-11-06T04:42:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868143.85 2024-11-06T04:42:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:24Z- waiting for 1730868147.85, time is 1730868144.69 2024-11-06T04:42:25Z- AHRS_M2 msg: 1730868145.06 epoch sec; seqNo:974 2024-11-06T04:42:25Z- AHRS_M2 msg: 0.11 phi; -0.19 theta; 1.57 psi 2024-11-06T04:42:25Z- handleDepth()- Depth_Keller msg: 1730868145.06 epoch sec; seqNo:1059; depth 49.00 2024-11-06T04:42:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868145.08 epoch sec; seqNo:1059; 4061663.61 north; 594280.75 east; 36.70 lat; -121.94 lon; 49.00 depth; utmZone: 10 2024-11-06T04:42:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868143.85 2024-11-06T04:42:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:25Z- waiting for 1730868147.85, time is 1730868145.08 2024-11-06T04:42:25Z- handleDepth()- Depth_Keller msg: 1730868145.48 epoch sec; seqNo:1060; depth 49.04 2024-11-06T04:42:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868145.49 epoch sec; seqNo:1060; 4061663.61 north; 594280.75 east; 36.70 lat; -121.94 lon; 49.04 depth; utmZone: 10 2024-11-06T04:42:25Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:42:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868143.85 2024-11-06T04:42:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:25Z- waiting for 1730868147.85, time is 1730868145.49 2024-11-06T04:42:25Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:42:25Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:42:25Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:42:25Z- AHRS_M2 msg: 1730868145.85 epoch sec; seqNo:975 2024-11-06T04:42:25Z- AHRS_M2 msg: 0.11 phi; -0.19 theta; 1.57 psi 2024-11-06T04:42:25Z- handleDepth()- Depth_Keller msg: 1730868145.89 epoch sec; seqNo:1061; depth 49.09 2024-11-06T04:42:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868145.90 epoch sec; seqNo:1061; 4061663.61 north; 594280.75 east; 36.70 lat; -121.94 lon; 49.09 depth; utmZone: 10 2024-11-06T04:42:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868143.85 2024-11-06T04:42:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:26Z- waiting for 1730868147.85, time is 1730868145.90 2024-11-06T04:42:26Z- AHRS_M2 msg: 1730868146.26 epoch sec; seqNo:976 2024-11-06T04:42:26Z- AHRS_M2 msg: 0.12 phi; -0.19 theta; 1.56 psi 2024-11-06T04:42:26Z- handleDepth()- Depth_Keller msg: 1730868146.27 epoch sec; seqNo:1062; depth 49.14 2024-11-06T04:42:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868146.27 epoch sec; seqNo:1062; 4061663.62 north; 594281.97 east; 36.70 lat; -121.94 lon; 49.14 depth; utmZone: 10 2024-11-06T04:42:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868143.85 2024-11-06T04:42:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:26Z- waiting for 1730868147.85, time is 1730868146.27 2024-11-06T04:42:26Z- AHRS_M2 msg: 1730868146.66 epoch sec; seqNo:977 2024-11-06T04:42:26Z- AHRS_M2 msg: 0.12 phi; -0.18 theta; 1.56 psi 2024-11-06T04:42:26Z- handleDepth()- Depth_Keller msg: 1730868146.67 epoch sec; seqNo:1063; depth 49.18 2024-11-06T04:42:26Z- handleDVL() - DVL msg epochMillisec: 1730868146.69 2024-11-06T04:42:26Z- handleDvl() - RDI_Pathfinder msg: 1730868146.69 sec; seqNo:350; #beams:4 2024-11-06T04:42:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 42.41 , 39.98 , 51.47 2024-11-06T04:42:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , 0.06 2024-11-06T04:42:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868146.71 epoch sec; seqNo:1063; 4061663.62 north; 594281.97 east; 36.70 lat; -121.94 lon; 49.18 depth; utmZone: 10 2024-11-06T04:42:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868143.85 2024-11-06T04:42:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:26Z- waiting for 1730868147.85, time is 1730868146.71 2024-11-06T04:42:27Z- AHRS_M2 msg: 1730868147.08 epoch sec; seqNo:978 2024-11-06T04:42:27Z- AHRS_M2 msg: 0.13 phi; -0.18 theta; 1.56 psi 2024-11-06T04:42:27Z- handleDepth()- Depth_Keller msg: 1730868147.09 epoch sec; seqNo:1064; depth 49.23 2024-11-06T04:42:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868147.12 epoch sec; seqNo:1064; 4061663.62 north; 594281.97 east; 36.70 lat; -121.94 lon; 49.23 depth; utmZone: 10 2024-11-06T04:42:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868143.85 2024-11-06T04:42:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:27Z- waiting for 1730868147.85, time is 1730868147.12 2024-11-06T04:42:27Z- AHRS_M2 msg: 1730868147.47 epoch sec; seqNo:979 2024-11-06T04:42:27Z- AHRS_M2 msg: 0.13 phi; -0.18 theta; 1.55 psi 2024-11-06T04:42:27Z- handleDepth()- Depth_Keller msg: 1730868147.48 epoch sec; seqNo:1065; depth 49.26 2024-11-06T04:42:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868147.49 epoch sec; seqNo:1065; 4061663.62 north; 594281.97 east; 36.70 lat; -121.94 lon; 49.26 depth; utmZone: 10 2024-11-06T04:42:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868143.85 2024-11-06T04:42:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:27Z- waiting for 1730868147.85, time is 1730868147.49 2024-11-06T04:42:27Z- AHRS_M2 msg: 1730868147.88 epoch sec; seqNo:980 2024-11-06T04:42:27Z- AHRS_M2 msg: 0.13 phi; -0.18 theta; 1.55 psi 2024-11-06T04:42:27Z- handleDepth()- Depth_Keller msg: 1730868147.89 epoch sec; seqNo:1066; depth 49.33 2024-11-06T04:42:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868147.91 epoch sec; seqNo:1066; 4061663.64 north; 594283.19 east; 36.70 lat; -121.94 lon; 49.33 depth; utmZone: 10 2024-11-06T04:42:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868143.85 2024-11-06T04:42:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:28Z- waiting for 1730868147.85, time is 1730868147.91 2024-11-06T04:42:28Z- LcmTrn::updateTrn() >>>> pose time 1730868147.91, meas time 1730868146.69 <<<< 2024-11-06T04:42:28Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868147.91 2024-11-06T04:42:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868146.69 sec, ping # 0; waiting for INS data more recent than 1730868143.85... 2024-11-06T04:42:28Z- TerrainNav::motionUpdate() - currEstimate = (4061664, 594283.19, 49.33) 2024-11-06T04:42:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:42:28Z- TerrainNav:: SubcloudNIS is 0.19, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:42:28Z- TNavPF::Projecting Measurements 2024-11-06T04:42:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:42:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:42:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:42:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:42:28Z- TNavFilter::4 good beams. 2024-11-06T04:42:28Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:42:28Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:42:28Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:42:28Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:42:28Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:42:28Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:42:28Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:42:28Z- TerrainMapOctree: (vehN, vehE ) = (4061663.62, 594282.08). 2024-11-06T04:42:28Z- TerrainMapOctree: Min Distance = 92.14. 2024-11-06T04:42:28Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:42:28Z- Forcing Subcloud Comparison 2024-11-06T04:42:28Z- Weighting particles with subcloud comparison 2024-11-06T04:42:28Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:28Z- meanExpectedMeasDiff: 0.501973 delta_rms_squared: 1.737327 alpha: 0.988619 2024-11-06T04:42:28Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:28Z- meanExpectedMeasDiff: 0.289650 delta_rms_squared: 1.308912 alpha: 0.984950 2024-11-06T04:42:28Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:28Z- meanExpectedMeasDiff: 0.490122 delta_rms_squared: 1.171600 alpha: 0.983216 2024-11-06T04:42:28Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:28Z- meanExpectedMeasDiff: -0.345201 delta_rms_squared: 2.168022 alpha: 0.990859 2024-11-06T04:42:28Z- Subcloud NIS: 0.183943 2024-11-06T04:42:28Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:42:28Z- TNavPF:: sumWeights = 0.633588 2024-11-06T04:42:28Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:42:28Z- TNavPF::First Term of Map Covariance Matrix is 0.52 2024-11-06T04:42:28Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.02 2024-11-06T04:42:28Z- TerrainNav::Resampling particle filter... 2024-11-06T04:42:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868146.69 sec. 2024-11-06T04:42:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:42:28Z- variances([0], [2], and psi_berg) = 0.162843 0.172499 0.000000 2024-11-06T04:42:28Z- publishEstimatesDecoder() - at 1730868147.91 sec 2024-11-06T04:42:28Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:42:28Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061607.85 594287.50 56.41 2024-11-06T04:42:28Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061607.56 594287.20 55.61 lat/lon: 36.70 -121.94 2024-11-06T04:42:28Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.17 0.75 0.00 2024-11-06T04:42:28Z- AHRS_M2 msg: 1730868148.28 epoch sec; seqNo:981 2024-11-06T04:42:28Z- AHRS_M2 msg: 0.13 phi; -0.18 theta; 1.55 psi 2024-11-06T04:42:28Z- handleDepth()- Depth_Keller msg: 1730868148.31 epoch sec; seqNo:1067; depth 49.37 2024-11-06T04:42:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868148.34 epoch sec; seqNo:1067; 4061663.64 north; 594283.19 east; 36.70 lat; -121.94 lon; 49.37 depth; utmZone: 10 2024-11-06T04:42:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868147.91 2024-11-06T04:42:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:28Z- waiting for 1730868151.91, time is 1730868148.34 2024-11-06T04:42:28Z- AHRS_M2 msg: 1730868148.68 epoch sec; seqNo:982 2024-11-06T04:42:28Z- AHRS_M2 msg: 0.13 phi; -0.18 theta; 1.55 psi 2024-11-06T04:42:28Z- handleDepth()- Depth_Keller msg: 1730868148.69 epoch sec; seqNo:1068; depth 49.43 2024-11-06T04:42:28Z- handleDVL() - DVL msg epochMillisec: 1730868148.71 2024-11-06T04:42:28Z- handleDvl() - RDI_Pathfinder msg: 1730868148.71 sec; seqNo:351; #beams:4 2024-11-06T04:42:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 41.97 , 39.98 , 51.47 2024-11-06T04:42:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , 0.07 2024-11-06T04:42:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868148.72 epoch sec; seqNo:1068; 4061664.02 north; 594283.18 east; 36.70 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T04:42:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868147.91 2024-11-06T04:42:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:28Z- waiting for 1730868151.91, time is 1730868148.72 2024-11-06T04:42:29Z- AHRS_M2 msg: 1730868149.09 epoch sec; seqNo:983 2024-11-06T04:42:29Z- AHRS_M2 msg: 0.13 phi; -0.18 theta; 1.55 psi 2024-11-06T04:42:29Z- handleDepth()- Depth_Keller msg: 1730868149.10 epoch sec; seqNo:1069; depth 49.47 2024-11-06T04:42:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868149.12 epoch sec; seqNo:1069; 4061664.03 north; 594284.40 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T04:42:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868147.91 2024-11-06T04:42:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:29Z- waiting for 1730868151.91, time is 1730868149.12 2024-11-06T04:42:29Z- AHRS_M2 msg: 1730868149.49 epoch sec; seqNo:984 2024-11-06T04:42:29Z- AHRS_M2 msg: 0.13 phi; -0.18 theta; 1.55 psi 2024-11-06T04:42:29Z- handleDepth()- Depth_Keller msg: 1730868149.50 epoch sec; seqNo:1070; depth 49.52 2024-11-06T04:42:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868149.51 epoch sec; seqNo:1070; 4061664.03 north; 594284.40 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:42:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868147.91 2024-11-06T04:42:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:29Z- waiting for 1730868151.91, time is 1730868149.51 2024-11-06T04:42:29Z- AHRS_M2 msg: 1730868149.90 epoch sec; seqNo:985 2024-11-06T04:42:29Z- AHRS_M2 msg: 0.13 phi; -0.18 theta; 1.55 psi 2024-11-06T04:42:29Z- handleDepth()- Depth_Keller msg: 1730868149.90 epoch sec; seqNo:1071; depth 49.56 2024-11-06T04:42:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868149.91 epoch sec; seqNo:1071; 4061664.03 north; 594284.40 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:42:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868147.91 2024-11-06T04:42:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:30Z- waiting for 1730868151.91, time is 1730868149.91 2024-11-06T04:42:30Z- AHRS_M2 msg: 1730868150.30 epoch sec; seqNo:986 2024-11-06T04:42:30Z- AHRS_M2 msg: 0.13 phi; -0.17 theta; 1.54 psi 2024-11-06T04:42:30Z- handleDepth()- Depth_Keller msg: 1730868150.31 epoch sec; seqNo:1072; depth 49.58 2024-11-06T04:42:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868150.32 epoch sec; seqNo:1072; 4061664.03 north; 594284.40 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:42:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868147.91 2024-11-06T04:42:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:30Z- waiting for 1730868151.91, time is 1730868150.32 2024-11-06T04:42:30Z- AHRS_M2 msg: 1730868150.71 epoch sec; seqNo:987 2024-11-06T04:42:30Z- AHRS_M2 msg: 0.13 phi; -0.17 theta; 1.54 psi 2024-11-06T04:42:30Z- handleDepth()- Depth_Keller msg: 1730868150.72 epoch sec; seqNo:1073; depth 49.64 2024-11-06T04:42:30Z- handleDVL() - DVL msg epochMillisec: 1730868150.74 2024-11-06T04:42:30Z- handleDvl() - RDI_Pathfinder msg: 1730868150.74 sec; seqNo:352; #beams:4 2024-11-06T04:42:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 41.59 , 39.96 , 50.77 2024-11-06T04:42:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.05 2024-11-06T04:42:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868150.76 epoch sec; seqNo:1073; 4061664.04 north; 594285.62 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:42:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868147.91 2024-11-06T04:42:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:30Z- waiting for 1730868151.91, time is 1730868150.76 2024-11-06T04:42:31Z- AHRS_M2 msg: 1730868151.11 epoch sec; seqNo:988 2024-11-06T04:42:31Z- AHRS_M2 msg: 0.13 phi; -0.17 theta; 1.54 psi 2024-11-06T04:42:31Z- handleDepth()- Depth_Keller msg: 1730868151.12 epoch sec; seqNo:1074; depth 49.67 2024-11-06T04:42:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868151.14 epoch sec; seqNo:1074; 4061664.04 north; 594285.62 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:42:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868147.91 2024-11-06T04:42:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:31Z- waiting for 1730868151.91, time is 1730868151.14 2024-11-06T04:42:31Z- AHRS_M2 msg: 1730868151.51 epoch sec; seqNo:989 2024-11-06T04:42:31Z- AHRS_M2 msg: 0.13 phi; -0.17 theta; 1.54 psi 2024-11-06T04:42:31Z- handleDepth()- Depth_Keller msg: 1730868151.52 epoch sec; seqNo:1075; depth 49.70 2024-11-06T04:42:31Z- handleDVL() - DVL msg epochMillisec: 1730868151.53 2024-11-06T04:42:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:42:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:42:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:42:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:42:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:42:31Z- handleDvl() - RDI_Pathfinder msg: 1730868151.53 sec; seqNo:353; #beams:0 2024-11-06T04:42:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:42:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.05 2024-11-06T04:42:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868151.54 epoch sec; seqNo:1075; 4061664.04 north; 594285.62 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T04:42:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868147.91 2024-11-06T04:42:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:31Z- waiting for 1730868151.91, time is 1730868151.54 2024-11-06T04:42:31Z- AHRS_M2 msg: 1730868151.92 epoch sec; seqNo:990 2024-11-06T04:42:31Z- AHRS_M2 msg: 0.13 phi; -0.17 theta; 1.54 psi 2024-11-06T04:42:31Z- handleDepth()- Depth_Keller msg: 1730868151.92 epoch sec; seqNo:1076; depth 49.74 2024-11-06T04:42:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868151.93 epoch sec; seqNo:1076; 4061664.04 north; 594285.62 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T04:42:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868147.91 2024-11-06T04:42:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:32Z- waiting for 1730868151.91, time is 1730868151.93 2024-11-06T04:42:32Z- LcmTrn::updateTrn() >>>> pose time 1730868151.93, meas time 1730868151.53 <<<< 2024-11-06T04:42:32Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868151.93 2024-11-06T04:42:32Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:42:32Z- TerrainNav::motionUpdate() - currEstimate = (4061664, 594285.62, 49.74) 2024-11-06T04:42:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:42:32Z- TerrainNav:: SubcloudNIS is 0.18, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:42:32Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:42:32Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:42:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:42:32Z- variances([0], [2], and psi_berg) = 0.163941 0.173567 0.000000 2024-11-06T04:42:32Z- publishEstimatesDecoder() - at 1730868151.93 sec 2024-11-06T04:42:32Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:42:32Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061608.27 594289.97 56.08 2024-11-06T04:42:32Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061607.96 594289.64 56.02 lat/lon: 36.70 -121.94 2024-11-06T04:42:32Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.16 0.17 0.98 0.00 2024-11-06T04:42:32Z- handleDepth()- Depth_Keller msg: 1730868152.35 epoch sec; seqNo:1077; depth 49.79 2024-11-06T04:42:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868152.39 epoch sec; seqNo:1077; 4061664.06 north; 594286.84 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:42:32Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:42:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868151.93 2024-11-06T04:42:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:32Z- waiting for 1730868155.93, time is 1730868152.39 2024-11-06T04:42:32Z- AHRS_M2 msg: 1730868152.73 epoch sec; seqNo:991 2024-11-06T04:42:32Z- AHRS_M2 msg: 0.13 phi; -0.16 theta; 1.54 psi 2024-11-06T04:42:32Z- handleDepth()- Depth_Keller msg: 1730868152.74 epoch sec; seqNo:1078; depth 49.83 2024-11-06T04:42:32Z- handleDVL() - DVL msg epochMillisec: 1730868152.76 2024-11-06T04:42:32Z- handleDvl() - RDI_Pathfinder msg: 1730868152.76 sec; seqNo:354; #beams:4 2024-11-06T04:42:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.31 , 41.19 , 39.76 , 49.75 2024-11-06T04:42:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , 0.01 2024-11-06T04:42:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868152.77 epoch sec; seqNo:1078; 4061664.06 north; 594286.84 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:42:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868151.93 2024-11-06T04:42:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:32Z- waiting for 1730868155.93, time is 1730868152.77 2024-11-06T04:42:33Z- AHRS_M2 msg: 1730868153.13 epoch sec; seqNo:992 2024-11-06T04:42:33Z- AHRS_M2 msg: 0.13 phi; -0.16 theta; 1.54 psi 2024-11-06T04:42:33Z- handleDepth()- Depth_Keller msg: 1730868153.13 epoch sec; seqNo:1079; depth 49.85 2024-11-06T04:42:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868153.15 epoch sec; seqNo:1079; 4061664.06 north; 594286.84 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:42:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868151.93 2024-11-06T04:42:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:33Z- waiting for 1730868155.93, time is 1730868153.15 2024-11-06T04:42:33Z- AHRS_M2 msg: 1730868153.53 epoch sec; seqNo:993 2024-11-06T04:42:33Z- AHRS_M2 msg: 0.13 phi; -0.16 theta; 1.54 psi 2024-11-06T04:42:33Z- handleDepth()- Depth_Keller msg: 1730868153.54 epoch sec; seqNo:1080; depth 49.90 2024-11-06T04:42:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868153.55 epoch sec; seqNo:1080; 4061664.07 north; 594288.06 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:42:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868151.93 2024-11-06T04:42:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:33Z- waiting for 1730868155.93, time is 1730868153.55 2024-11-06T04:42:33Z- AHRS_M2 msg: 1730868153.93 epoch sec; seqNo:994 2024-11-06T04:42:33Z- AHRS_M2 msg: 0.13 phi; -0.15 theta; 1.54 psi 2024-11-06T04:42:33Z- handleDepth()- Depth_Keller msg: 1730868153.94 epoch sec; seqNo:1081; depth 49.92 2024-11-06T04:42:33Z- handleDVL() - DVL msg epochMillisec: 1730868153.94 2024-11-06T04:42:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:42:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:42:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:42:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:42:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:42:33Z- handleDvl() - RDI_Pathfinder msg: 1730868153.94 sec; seqNo:355; #beams:0 2024-11-06T04:42:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:42:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , 0.01 2024-11-06T04:42:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868153.95 epoch sec; seqNo:1081; 4061664.07 north; 594288.06 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:42:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868151.93 2024-11-06T04:42:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:34Z- waiting for 1730868155.93, time is 1730868153.95 2024-11-06T04:42:34Z- AHRS_M2 msg: 1730868154.36 epoch sec; seqNo:995 2024-11-06T04:42:34Z- AHRS_M2 msg: 0.13 phi; -0.15 theta; 1.53 psi 2024-11-06T04:42:34Z- handleDepth()- Depth_Keller msg: 1730868154.38 epoch sec; seqNo:1082; depth 49.97 2024-11-06T04:42:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868154.41 epoch sec; seqNo:1082; 4061664.45 north; 594288.06 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T04:42:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868151.93 2024-11-06T04:42:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:34Z- waiting for 1730868155.93, time is 1730868154.41 2024-11-06T04:42:34Z- handleDepth()- Depth_Keller msg: 1730868154.76 epoch sec; seqNo:1083; depth 50.00 2024-11-06T04:42:34Z- handleDVL() - DVL msg epochMillisec: 1730868154.78 2024-11-06T04:42:34Z- handleDvl() - RDI_Pathfinder msg: 1730868154.78 sec; seqNo:356; #beams:4 2024-11-06T04:42:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.69 , 40.78 , 39.56 , 48.94 2024-11-06T04:42:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , -0.02 2024-11-06T04:42:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868154.80 epoch sec; seqNo:1083; 4061664.45 north; 594288.06 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T04:42:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868151.93 2024-11-06T04:42:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:34Z- waiting for 1730868155.93, time is 1730868154.80 2024-11-06T04:42:35Z- AHRS_M2 msg: 1730868155.15 epoch sec; seqNo:996 2024-11-06T04:42:35Z- AHRS_M2 msg: 0.13 phi; -0.14 theta; 1.53 psi 2024-11-06T04:42:35Z- handleDepth()- Depth_Keller msg: 1730868155.15 epoch sec; seqNo:1084; depth 50.03 2024-11-06T04:42:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868155.17 epoch sec; seqNo:1084; 4061664.46 north; 594289.28 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T04:42:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868151.93 2024-11-06T04:42:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:35Z- waiting for 1730868155.93, time is 1730868155.17 2024-11-06T04:42:35Z- AHRS_M2 msg: 1730868155.55 epoch sec; seqNo:997 2024-11-06T04:42:35Z- AHRS_M2 msg: 0.13 phi; -0.14 theta; 1.53 psi 2024-11-06T04:42:35Z- handleDepth()- Depth_Keller msg: 1730868155.56 epoch sec; seqNo:1085; depth 50.06 2024-11-06T04:42:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868155.56 epoch sec; seqNo:1085; 4061664.46 north; 594289.28 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T04:42:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868151.93 2024-11-06T04:42:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:35Z- waiting for 1730868155.93, time is 1730868155.56 2024-11-06T04:42:35Z- AHRS_M2 msg: 1730868155.95 epoch sec; seqNo:998 2024-11-06T04:42:35Z- AHRS_M2 msg: 0.13 phi; -0.14 theta; 1.53 psi 2024-11-06T04:42:35Z- handleDepth()- Depth_Keller msg: 1730868155.97 epoch sec; seqNo:1086; depth 50.11 2024-11-06T04:42:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868155.99 epoch sec; seqNo:1086; 4061664.46 north; 594289.28 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T04:42:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868151.93 2024-11-06T04:42:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:36Z- waiting for 1730868155.93, time is 1730868155.99 2024-11-06T04:42:36Z- LcmTrn::updateTrn() >>>> pose time 1730868155.99, meas time 1730868154.78 <<<< 2024-11-06T04:42:36Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868155.99 2024-11-06T04:42:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868154.78 sec, ping # 0; waiting for INS data more recent than 1730868151.93... 2024-11-06T04:42:36Z- TerrainNav::motionUpdate() - currEstimate = (4061664, 594289.28, 50.11) 2024-11-06T04:42:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:42:36Z- TerrainNav:: SubcloudNIS is 0.18, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:42:36Z- TNavPF::Projecting Measurements 2024-11-06T04:42:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:42:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:42:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:42:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:42:36Z- TNavFilter::4 good beams. 2024-11-06T04:42:36Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:42:36Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:42:36Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:42:36Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:42:36Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:42:36Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:42:36Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:42:36Z- TerrainMapOctree: (vehN, vehE ) = (4061664.34, 594288.20). 2024-11-06T04:42:36Z- TerrainMapOctree: Min Distance = 93.79. 2024-11-06T04:42:36Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:42:36Z- Forcing Subcloud Comparison 2024-11-06T04:42:36Z- Weighting particles with subcloud comparison 2024-11-06T04:42:36Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:36Z- meanExpectedMeasDiff: 0.241961 delta_rms_squared: 2.018194 alpha: 0.990187 2024-11-06T04:42:36Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:36Z- meanExpectedMeasDiff: -0.106290 delta_rms_squared: 1.478384 alpha: 0.986652 2024-11-06T04:42:36Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:36Z- meanExpectedMeasDiff: 0.227486 delta_rms_squared: 1.215670 alpha: 0.983814 2024-11-06T04:42:36Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:36Z- meanExpectedMeasDiff: 0.719919 delta_rms_squared: 2.059942 alpha: 0.990384 2024-11-06T04:42:36Z- Subcloud NIS: 0.164146 2024-11-06T04:42:36Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:42:36Z- TNavPF:: sumWeights = 0.622318 2024-11-06T04:42:36Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:42:36Z- TNavPF::First Term of Map Covariance Matrix is 0.53 2024-11-06T04:42:36Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T04:42:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868154.78 sec. 2024-11-06T04:42:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:42:36Z- variances([0], [2], and psi_berg) = 0.165263 0.173401 0.000000 2024-11-06T04:42:36Z- publishEstimatesDecoder() - at 1730868155.99 sec 2024-11-06T04:42:36Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:42:36Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061608.57 594293.61 57.06 2024-11-06T04:42:36Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061608.39 594293.29 56.26 lat/lon: 36.70 -121.94 2024-11-06T04:42:36Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.17 0.77 0.00 2024-11-06T04:42:36Z- AHRS_M2 msg: 1730868156.41 epoch sec; seqNo:999 2024-11-06T04:42:36Z- AHRS_M2 msg: 0.13 phi; -0.14 theta; 1.53 psi 2024-11-06T04:42:36Z- handleDepth()- Depth_Keller msg: 1730868156.43 epoch sec; seqNo:1087; depth 50.13 2024-11-06T04:42:36Z- handleDVL() - DVL msg epochMillisec: 1730868156.47 2024-11-06T04:42:36Z- handleDvl() - RDI_Pathfinder msg: 1730868156.47 sec; seqNo:357; #beams:4 2024-11-06T04:42:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 40.17 , 39.76 , 47.30 2024-11-06T04:42:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.07 , -0.04 2024-11-06T04:42:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868156.48 epoch sec; seqNo:1087; 4061664.48 north; 594290.50 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T04:42:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868155.99 2024-11-06T04:42:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:36Z- waiting for 1730868159.99, time is 1730868156.48 2024-11-06T04:42:36Z- AHRS_M2 msg: 1730868156.77 epoch sec; seqNo:1000 2024-11-06T04:42:36Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 1.53 psi 2024-11-06T04:42:36Z- handleDepth()- Depth_Keller msg: 1730868156.77 epoch sec; seqNo:1088; depth 50.19 2024-11-06T04:42:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868156.79 epoch sec; seqNo:1088; 4061664.48 north; 594290.50 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:42:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868155.99 2024-11-06T04:42:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:36Z- waiting for 1730868159.99, time is 1730868156.79 2024-11-06T04:42:37Z- AHRS_M2 msg: 1730868157.17 epoch sec; seqNo:1001 2024-11-06T04:42:37Z- AHRS_M2 msg: 0.13 phi; -0.13 theta; 1.53 psi 2024-11-06T04:42:37Z- handleDepth()- Depth_Keller msg: 1730868157.18 epoch sec; seqNo:1089; depth 50.22 2024-11-06T04:42:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868157.19 epoch sec; seqNo:1089; 4061664.48 north; 594290.50 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T04:42:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868155.99 2024-11-06T04:42:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:37Z- waiting for 1730868159.99, time is 1730868157.19 2024-11-06T04:42:37Z- AHRS_M2 msg: 1730868157.57 epoch sec; seqNo:1002 2024-11-06T04:42:37Z- AHRS_M2 msg: 0.14 phi; -0.13 theta; 1.53 psi 2024-11-06T04:42:37Z- handleDepth()- Depth_Keller msg: 1730868157.58 epoch sec; seqNo:1090; depth 50.24 2024-11-06T04:42:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868157.58 epoch sec; seqNo:1090; 4061664.48 north; 594290.50 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:42:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868155.99 2024-11-06T04:42:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:37Z- waiting for 1730868159.99, time is 1730868157.58 2024-11-06T04:42:37Z- AHRS_M2 msg: 1730868157.97 epoch sec; seqNo:1003 2024-11-06T04:42:37Z- AHRS_M2 msg: 0.14 phi; -0.13 theta; 1.52 psi 2024-11-06T04:42:37Z- handleDepth()- Depth_Keller msg: 1730868157.99 epoch sec; seqNo:1091; depth 50.26 2024-11-06T04:42:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868157.99 epoch sec; seqNo:1091; 4061664.49 north; 594291.72 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:42:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868155.99 2024-11-06T04:42:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:38Z- waiting for 1730868159.99, time is 1730868157.99 2024-11-06T04:42:38Z- AHRS_M2 msg: 1730868158.41 epoch sec; seqNo:1004 2024-11-06T04:42:38Z- AHRS_M2 msg: 0.13 phi; -0.12 theta; 1.52 psi 2024-11-06T04:42:38Z- handleDepth()- Depth_Keller msg: 1730868158.41 epoch sec; seqNo:1092; depth 50.32 2024-11-06T04:42:38Z- handleDVL() - DVL msg epochMillisec: 1730868158.44 2024-11-06T04:42:38Z- handleDvl() - RDI_Pathfinder msg: 1730868158.44 sec; seqNo:358; #beams:4 2024-11-06T04:42:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 39.96 , 39.96 , 46.49 2024-11-06T04:42:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.06 , 0.01 2024-11-06T04:42:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868158.45 epoch sec; seqNo:1092; 4061664.87 north; 594291.72 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:42:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868155.99 2024-11-06T04:42:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:38Z- waiting for 1730868159.99, time is 1730868158.45 2024-11-06T04:42:38Z- handleDepth()- Depth_Keller msg: 1730868158.81 epoch sec; seqNo:1093; depth 50.35 2024-11-06T04:42:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868158.84 epoch sec; seqNo:1093; 4061664.87 north; 594291.72 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:42:38Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:42:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868155.99 2024-11-06T04:42:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:38Z- waiting for 1730868159.99, time is 1730868158.84 2024-11-06T04:42:39Z- AHRS_M2 msg: 1730868159.19 epoch sec; seqNo:1005 2024-11-06T04:42:39Z- AHRS_M2 msg: 0.13 phi; -0.12 theta; 1.52 psi 2024-11-06T04:42:39Z- handleDepth()- Depth_Keller msg: 1730868159.21 epoch sec; seqNo:1094; depth 50.40 2024-11-06T04:42:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868159.22 epoch sec; seqNo:1094; 4061664.87 north; 594291.72 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T04:42:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868155.99 2024-11-06T04:42:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:39Z- waiting for 1730868159.99, time is 1730868159.22 2024-11-06T04:42:39Z- AHRS_M2 msg: 1730868159.60 epoch sec; seqNo:1006 2024-11-06T04:42:39Z- AHRS_M2 msg: 0.13 phi; -0.11 theta; 1.51 psi 2024-11-06T04:42:39Z- handleDepth()- Depth_Keller msg: 1730868159.60 epoch sec; seqNo:1095; depth 50.43 2024-11-06T04:42:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868159.61 epoch sec; seqNo:1095; 4061664.88 north; 594292.94 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:42:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868155.99 2024-11-06T04:42:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:39Z- waiting for 1730868159.99, time is 1730868159.61 2024-11-06T04:42:40Z- AHRS_M2 msg: 1730868160.00 epoch sec; seqNo:1007 2024-11-06T04:42:40Z- AHRS_M2 msg: 0.13 phi; -0.11 theta; 1.51 psi 2024-11-06T04:42:40Z- handleDepth()- Depth_Keller msg: 1730868160.00 epoch sec; seqNo:1096; depth 50.46 2024-11-06T04:42:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868160.01 epoch sec; seqNo:1096; 4061664.88 north; 594292.94 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T04:42:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868155.99 2024-11-06T04:42:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:40Z- waiting for 1730868159.99, time is 1730868160.01 2024-11-06T04:42:40Z- LcmTrn::updateTrn() >>>> pose time 1730868160.01, meas time 1730868158.44 <<<< 2024-11-06T04:42:40Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868160.01 2024-11-06T04:42:40Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868158.44 sec, ping # 0; waiting for INS data more recent than 1730868155.99... 2024-11-06T04:42:40Z- TerrainNav::motionUpdate() - currEstimate = (4061665, 594292.94, 50.46) 2024-11-06T04:42:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:42:40Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:42:40Z- TNavPF::Projecting Measurements 2024-11-06T04:42:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:42:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:42:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:42:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:42:40Z- TNavFilter::4 good beams. 2024-11-06T04:42:40Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:42:40Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:42:40Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:42:40Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:42:40Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:42:40Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:42:40Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:42:40Z- TerrainMapOctree: (vehN, vehE ) = (4061664.72, 594291.51). 2024-11-06T04:42:40Z- TerrainMapOctree: Min Distance = 94.85. 2024-11-06T04:42:40Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:42:40Z- Forcing Subcloud Comparison 2024-11-06T04:42:40Z- Weighting particles with subcloud comparison 2024-11-06T04:42:40Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:40Z- meanExpectedMeasDiff: 0.255359 delta_rms_squared: 1.514154 alpha: 0.986963 2024-11-06T04:42:40Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:40Z- meanExpectedMeasDiff: 0.197260 delta_rms_squared: 1.941075 alpha: 0.989802 2024-11-06T04:42:40Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:40Z- meanExpectedMeasDiff: 1.405072 delta_rms_squared: 1.190035 alpha: 0.983472 2024-11-06T04:42:40Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:40Z- meanExpectedMeasDiff: -0.550746 delta_rms_squared: 1.594923 alpha: 0.987616 2024-11-06T04:42:40Z- Subcloud NIS: 0.164511 2024-11-06T04:42:40Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:42:40Z- TNavPF:: sumWeights = 0.506932 2024-11-06T04:42:40Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:42:40Z- TNavPF::First Term of Map Covariance Matrix is 0.34 2024-11-06T04:42:40Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.04 2024-11-06T04:42:40Z- TerrainNav::Resampling particle filter... 2024-11-06T04:42:40Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868158.44 sec. 2024-11-06T04:42:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:42:40Z- variances([0], [2], and psi_berg) = 0.169741 0.178552 0.000000 2024-11-06T04:42:40Z- publishEstimatesDecoder() - at 1730868160.01 sec 2024-11-06T04:42:40Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:42:40Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061609.12 594297.32 54.10 2024-11-06T04:42:40Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061608.81 594296.96 56.47 lat/lon: 36.70 -121.94 2024-11-06T04:42:40Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.18 0.69 0.00 2024-11-06T04:42:40Z- handleDepth()- Depth_Keller msg: 1730868160.48 epoch sec; seqNo:1097; depth 50.51 2024-11-06T04:42:40Z- handleDVL() - DVL msg epochMillisec: 1730868160.52 2024-11-06T04:42:40Z- handleDvl() - RDI_Pathfinder msg: 1730868160.52 sec; seqNo:359; #beams:4 2024-11-06T04:42:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.08 , 39.76 , 39.76 , 45.67 2024-11-06T04:42:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.05 , 0.08 2024-11-06T04:42:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868160.54 epoch sec; seqNo:1097; 4061664.88 north; 594292.94 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T04:42:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868160.01 2024-11-06T04:42:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:40Z- waiting for 1730868164.01, time is 1730868160.54 2024-11-06T04:42:40Z- AHRS_M2 msg: 1730868160.81 epoch sec; seqNo:1008 2024-11-06T04:42:40Z- AHRS_M2 msg: 0.13 phi; -0.11 theta; 1.51 psi 2024-11-06T04:42:40Z- handleDepth()- Depth_Keller msg: 1730868160.82 epoch sec; seqNo:1098; depth 50.54 2024-11-06T04:42:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868160.84 epoch sec; seqNo:1098; 4061664.88 north; 594292.94 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T04:42:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868160.01 2024-11-06T04:42:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:40Z- waiting for 1730868164.01, time is 1730868160.84 2024-11-06T04:42:41Z- AHRS_M2 msg: 1730868161.21 epoch sec; seqNo:1009 2024-11-06T04:42:41Z- AHRS_M2 msg: 0.14 phi; -0.11 theta; 1.51 psi 2024-11-06T04:42:41Z- handleDepth()- Depth_Keller msg: 1730868161.22 epoch sec; seqNo:1099; depth 50.56 2024-11-06T04:42:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868161.23 epoch sec; seqNo:1099; 4061664.89 north; 594294.16 east; 36.70 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T04:42:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868160.01 2024-11-06T04:42:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:41Z- waiting for 1730868164.01, time is 1730868161.23 2024-11-06T04:42:41Z- AHRS_M2 msg: 1730868161.61 epoch sec; seqNo:1010 2024-11-06T04:42:41Z- AHRS_M2 msg: 0.14 phi; -0.11 theta; 1.51 psi 2024-11-06T04:42:41Z- handleDepth()- Depth_Keller msg: 1730868161.62 epoch sec; seqNo:1100; depth 50.59 2024-11-06T04:42:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868161.64 epoch sec; seqNo:1100; 4061664.89 north; 594294.16 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T04:42:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868160.01 2024-11-06T04:42:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:41Z- waiting for 1730868164.01, time is 1730868161.64 2024-11-06T04:42:42Z- AHRS_M2 msg: 1730868162.02 epoch sec; seqNo:1011 2024-11-06T04:42:42Z- AHRS_M2 msg: 0.14 phi; -0.11 theta; 1.51 psi 2024-11-06T04:42:42Z- handleDepth()- Depth_Keller msg: 1730868162.03 epoch sec; seqNo:1101; depth 50.62 2024-11-06T04:42:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868162.03 epoch sec; seqNo:1101; 4061665.27 north; 594294.15 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T04:42:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868160.01 2024-11-06T04:42:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:42Z- waiting for 1730868164.01, time is 1730868162.03 2024-11-06T04:42:42Z- AHRS_M2 msg: 1730868162.42 epoch sec; seqNo:1012 2024-11-06T04:42:42Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 1.51 psi 2024-11-06T04:42:42Z- handleDepth()- Depth_Keller msg: 1730868162.42 epoch sec; seqNo:1102; depth 50.66 2024-11-06T04:42:42Z- handleDVL() - DVL msg epochMillisec: 1730868162.44 2024-11-06T04:42:42Z- handleDvl() - RDI_Pathfinder msg: 1730868162.44 sec; seqNo:360; #beams:4 2024-11-06T04:42:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.08 , 39.76 , 39.35 , 45.67 2024-11-06T04:42:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.12 2024-11-06T04:42:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868162.48 epoch sec; seqNo:1102; 4061665.27 north; 594294.15 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T04:42:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868160.01 2024-11-06T04:42:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:42Z- waiting for 1730868164.01, time is 1730868162.48 2024-11-06T04:42:42Z- AHRS_M2 msg: 1730868162.83 epoch sec; seqNo:1013 2024-11-06T04:42:42Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 1.51 psi 2024-11-06T04:42:42Z- handleDepth()- Depth_Keller msg: 1730868162.84 epoch sec; seqNo:1103; depth 50.69 2024-11-06T04:42:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868162.86 epoch sec; seqNo:1103; 4061665.29 north; 594295.37 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T04:42:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868160.01 2024-11-06T04:42:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:43Z- waiting for 1730868164.01, time is 1730868162.86 2024-11-06T04:42:43Z- AHRS_M2 msg: 1730868163.23 epoch sec; seqNo:1014 2024-11-06T04:42:43Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 1.51 psi 2024-11-06T04:42:43Z- handleDepth()- Depth_Keller msg: 1730868163.25 epoch sec; seqNo:1104; depth 50.72 2024-11-06T04:42:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868163.26 epoch sec; seqNo:1104; 4061665.29 north; 594295.37 east; 36.70 lat; -121.94 lon; 50.72 depth; utmZone: 10 2024-11-06T04:42:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868160.01 2024-11-06T04:42:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:43Z- waiting for 1730868164.01, time is 1730868163.26 2024-11-06T04:42:43Z- handleDepth()- Depth_Keller msg: 1730868163.65 epoch sec; seqNo:1105; depth 50.73 2024-11-06T04:42:43Z- handleDVL() - DVL msg epochMillisec: 1730868163.66 2024-11-06T04:42:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:42:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:42:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:42:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:42:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:42:43Z- handleDvl() - RDI_Pathfinder msg: 1730868163.66 sec; seqNo:361; #beams:0 2024-11-06T04:42:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:42:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.12 2024-11-06T04:42:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868163.66 epoch sec; seqNo:1105; 4061665.29 north; 594295.37 east; 36.70 lat; -121.94 lon; 50.73 depth; utmZone: 10 2024-11-06T04:42:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868160.01 2024-11-06T04:42:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:43Z- waiting for 1730868164.01, time is 1730868163.66 2024-11-06T04:42:44Z- AHRS_M2 msg: 1730868164.03 epoch sec; seqNo:1015 2024-11-06T04:42:44Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 1.51 psi 2024-11-06T04:42:44Z- handleDepth()- Depth_Keller msg: 1730868164.04 epoch sec; seqNo:1106; depth 50.76 2024-11-06T04:42:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868164.05 epoch sec; seqNo:1106; 4061665.29 north; 594295.37 east; 36.70 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T04:42:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868160.01 2024-11-06T04:42:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:44Z- waiting for 1730868164.01, time is 1730868164.05 2024-11-06T04:42:44Z- LcmTrn::updateTrn() >>>> pose time 1730868164.05, meas time 1730868163.66 <<<< 2024-11-06T04:42:44Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868164.05 2024-11-06T04:42:44Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:42:44Z- TerrainNav::motionUpdate() - currEstimate = (4061665, 594295.37, 50.76) 2024-11-06T04:42:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:42:44Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:42:44Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:42:44Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:42:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:42:44Z- variances([0], [2], and psi_berg) = 0.170406 0.179928 0.000000 2024-11-06T04:42:44Z- publishEstimatesDecoder() - at 1730868164.05 sec 2024-11-06T04:42:44Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:42:44Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061609.56 594299.75 54.50 2024-11-06T04:42:44Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061609.22 594299.40 56.76 lat/lon: 36.70 -121.94 2024-11-06T04:42:44Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.18 0.94 0.00 2024-11-06T04:42:44Z- AHRS_M2 msg: 1730868164.44 epoch sec; seqNo:1016 2024-11-06T04:42:44Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 1.52 psi 2024-11-06T04:42:44Z- handleDepth()- Depth_Keller msg: 1730868164.45 epoch sec; seqNo:1107; depth 50.77 2024-11-06T04:42:44Z- handleDVL() - DVL msg epochMillisec: 1730868164.47 2024-11-06T04:42:44Z- handleDvl() - RDI_Pathfinder msg: 1730868164.47 sec; seqNo:362; #beams:4 2024-11-06T04:42:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.29 , 39.76 , 39.35 , 45.47 2024-11-06T04:42:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.02 , 0.07 2024-11-06T04:42:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868164.48 epoch sec; seqNo:1107; 4061665.30 north; 594296.59 east; 36.70 lat; -121.94 lon; 50.77 depth; utmZone: 10 2024-11-06T04:42:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868164.05 2024-11-06T04:42:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:44Z- waiting for 1730868168.05, time is 1730868164.48 2024-11-06T04:42:44Z- AHRS_M2 msg: 1730868164.85 epoch sec; seqNo:1017 2024-11-06T04:42:44Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 1.52 psi 2024-11-06T04:42:44Z- handleDepth()- Depth_Keller msg: 1730868164.85 epoch sec; seqNo:1108; depth 50.80 2024-11-06T04:42:44Z- handleDVL() - DVL msg epochMillisec: 1730868164.85 2024-11-06T04:42:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:42:44Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:42:44Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:42:44Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:42:44Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:42:44Z- handleDvl() - RDI_Pathfinder msg: 1730868164.85 sec; seqNo:363; #beams:0 2024-11-06T04:42:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:42:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.02 , 0.07 2024-11-06T04:42:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868164.88 epoch sec; seqNo:1108; 4061665.30 north; 594296.59 east; 36.70 lat; -121.94 lon; 50.80 depth; utmZone: 10 2024-11-06T04:42:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868164.05 2024-11-06T04:42:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:45Z- waiting for 1730868168.05, time is 1730868164.88 2024-11-06T04:42:45Z- AHRS_M2 msg: 1730868165.25 epoch sec; seqNo:1018 2024-11-06T04:42:45Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 1.52 psi 2024-11-06T04:42:45Z- handleDepth()- Depth_Keller msg: 1730868165.25 epoch sec; seqNo:1109; depth 50.82 2024-11-06T04:42:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868165.26 epoch sec; seqNo:1109; 4061665.30 north; 594296.59 east; 36.70 lat; -121.94 lon; 50.82 depth; utmZone: 10 2024-11-06T04:42:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868164.05 2024-11-06T04:42:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:45Z- waiting for 1730868168.05, time is 1730868165.26 2024-11-06T04:42:45Z- handleDepth()- Depth_Keller msg: 1730868165.67 epoch sec; seqNo:1110; depth 50.82 2024-11-06T04:42:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868165.68 epoch sec; seqNo:1110; 4061665.31 north; 594297.81 east; 36.70 lat; -121.94 lon; 50.82 depth; utmZone: 10 2024-11-06T04:42:45Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:42:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868164.05 2024-11-06T04:42:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:45Z- waiting for 1730868168.05, time is 1730868165.68 2024-11-06T04:42:46Z- AHRS_M2 msg: 1730868166.06 epoch sec; seqNo:1019 2024-11-06T04:42:46Z- AHRS_M2 msg: 0.14 phi; -0.09 theta; 1.52 psi 2024-11-06T04:42:46Z- handleDepth()- Depth_Keller msg: 1730868166.07 epoch sec; seqNo:1111; depth 50.85 2024-11-06T04:42:46Z- handleDVL() - DVL msg epochMillisec: 1730868166.07 2024-11-06T04:42:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:42:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:42:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:42:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:42:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:42:46Z- handleDvl() - RDI_Pathfinder msg: 1730868166.07 sec; seqNo:364; #beams:0 2024-11-06T04:42:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:42:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.02 , 0.07 2024-11-06T04:42:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868166.08 epoch sec; seqNo:1111; 4061665.69 north; 594297.81 east; 36.70 lat; -121.94 lon; 50.85 depth; utmZone: 10 2024-11-06T04:42:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868164.05 2024-11-06T04:42:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:46Z- waiting for 1730868168.05, time is 1730868166.08 2024-11-06T04:42:46Z- AHRS_M2 msg: 1730868166.46 epoch sec; seqNo:1020 2024-11-06T04:42:46Z- AHRS_M2 msg: 0.14 phi; -0.09 theta; 1.53 psi 2024-11-06T04:42:46Z- handleDepth()- Depth_Keller msg: 1730868166.47 epoch sec; seqNo:1112; depth 50.86 2024-11-06T04:42:46Z- handleDVL() - DVL msg epochMillisec: 1730868166.50 2024-11-06T04:42:46Z- handleDvl() - RDI_Pathfinder msg: 1730868166.50 sec; seqNo:365; #beams:4 2024-11-06T04:42:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.88 , 39.56 , 39.76 , 44.86 2024-11-06T04:42:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , -0.05 2024-11-06T04:42:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868166.51 epoch sec; seqNo:1112; 4061665.69 north; 594297.81 east; 36.70 lat; -121.94 lon; 50.86 depth; utmZone: 10 2024-11-06T04:42:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868164.05 2024-11-06T04:42:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:46Z- waiting for 1730868168.05, time is 1730868166.51 2024-11-06T04:42:46Z- AHRS_M2 msg: 1730868166.87 epoch sec; seqNo:1021 2024-11-06T04:42:46Z- AHRS_M2 msg: 0.14 phi; -0.09 theta; 1.53 psi 2024-11-06T04:42:46Z- handleDepth()- Depth_Keller msg: 1730868166.87 epoch sec; seqNo:1113; depth 50.87 2024-11-06T04:42:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868166.89 epoch sec; seqNo:1113; 4061665.71 north; 594299.03 east; 36.70 lat; -121.94 lon; 50.87 depth; utmZone: 10 2024-11-06T04:42:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868164.05 2024-11-06T04:42:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:47Z- waiting for 1730868168.05, time is 1730868166.89 2024-11-06T04:42:47Z- AHRS_M2 msg: 1730868167.28 epoch sec; seqNo:1022 2024-11-06T04:42:47Z- AHRS_M2 msg: 0.14 phi; -0.09 theta; 1.53 psi 2024-11-06T04:42:47Z- handleDepth()- Depth_Keller msg: 1730868167.28 epoch sec; seqNo:1114; depth 50.89 2024-11-06T04:42:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868167.29 epoch sec; seqNo:1114; 4061665.71 north; 594299.03 east; 36.70 lat; -121.94 lon; 50.89 depth; utmZone: 10 2024-11-06T04:42:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868164.05 2024-11-06T04:42:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:47Z- waiting for 1730868168.05, time is 1730868167.29 2024-11-06T04:42:47Z- AHRS_M2 msg: 1730868167.67 epoch sec; seqNo:1023 2024-11-06T04:42:47Z- AHRS_M2 msg: 0.14 phi; -0.09 theta; 1.53 psi 2024-11-06T04:42:47Z- handleDepth()- Depth_Keller msg: 1730868167.68 epoch sec; seqNo:1115; depth 50.89 2024-11-06T04:42:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868167.69 epoch sec; seqNo:1115; 4061665.71 north; 594299.03 east; 36.70 lat; -121.94 lon; 50.89 depth; utmZone: 10 2024-11-06T04:42:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868164.05 2024-11-06T04:42:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:47Z- waiting for 1730868168.05, time is 1730868167.69 2024-11-06T04:42:48Z- AHRS_M2 msg: 1730868168.08 epoch sec; seqNo:1024 2024-11-06T04:42:48Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 1.53 psi 2024-11-06T04:42:48Z- handleDepth()- Depth_Keller msg: 1730868168.08 epoch sec; seqNo:1116; depth 50.89 2024-11-06T04:42:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868168.09 epoch sec; seqNo:1116; 4061665.72 north; 594300.25 east; 36.70 lat; -121.94 lon; 50.89 depth; utmZone: 10 2024-11-06T04:42:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868164.05 2024-11-06T04:42:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:48Z- waiting for 1730868168.05, time is 1730868168.09 2024-11-06T04:42:48Z- LcmTrn::updateTrn() >>>> pose time 1730868168.09, meas time 1730868166.50 <<<< 2024-11-06T04:42:48Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868168.09 2024-11-06T04:42:48Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868166.50 sec, ping # 0; waiting for INS data more recent than 1730868164.05... 2024-11-06T04:42:48Z- TerrainNav::motionUpdate() - currEstimate = (4061666, 594300.25, 50.89) 2024-11-06T04:42:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:42:48Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:42:48Z- TNavPF::Projecting Measurements 2024-11-06T04:42:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:42:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:42:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:42:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:42:48Z- TNavFilter::4 good beams. 2024-11-06T04:42:48Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:42:48Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:42:48Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:42:48Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:42:48Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:42:48Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:42:48Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:42:48Z- TerrainMapOctree: (vehN, vehE ) = (4061665.55, 594298.33). 2024-11-06T04:42:48Z- TerrainMapOctree: Min Distance = 97.30. 2024-11-06T04:42:48Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:42:48Z- Forcing Subcloud Comparison 2024-11-06T04:42:48Z- Weighting particles with subcloud comparison 2024-11-06T04:42:48Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:48Z- meanExpectedMeasDiff: 1.794426 delta_rms_squared: 1.842837 alpha: 0.989264 2024-11-06T04:42:48Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:48Z- meanExpectedMeasDiff: 0.822697 delta_rms_squared: 1.385894 alpha: 0.985774 2024-11-06T04:42:48Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:48Z- meanExpectedMeasDiff: 1.055205 delta_rms_squared: 1.308628 alpha: 0.984947 2024-11-06T04:42:48Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:48Z- meanExpectedMeasDiff: -0.716070 delta_rms_squared: 1.819899 alpha: 0.989130 2024-11-06T04:42:48Z- Subcloud NIS: 0.169783 2024-11-06T04:42:48Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:42:48Z- TNavPF:: sumWeights = 0.411207 2024-11-06T04:42:48Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:42:48Z- TNavPF::First Term of Map Covariance Matrix is 0.65 2024-11-06T04:42:48Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T04:42:48Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868166.50 sec. 2024-11-06T04:42:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:42:48Z- variances([0], [2], and psi_berg) = 0.173820 0.174916 0.000000 2024-11-06T04:42:48Z- publishEstimatesDecoder() - at 1730868168.09 sec 2024-11-06T04:42:48Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:42:48Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061609.00 594304.86 56.56 2024-11-06T04:42:48Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061609.65 594304.33 56.55 lat/lon: 36.70 -121.94 2024-11-06T04:42:48Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.17 0.77 0.00 2024-11-06T04:42:48Z- AHRS_M2 msg: 1730868168.48 epoch sec; seqNo:1025 2024-11-06T04:42:48Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 1.53 psi 2024-11-06T04:42:48Z- handleDepth()- Depth_Keller msg: 1730868168.48 epoch sec; seqNo:1117; depth 50.91 2024-11-06T04:42:48Z- handleDVL() - DVL msg epochMillisec: 1730868168.50 2024-11-06T04:42:48Z- handleDvl() - RDI_Pathfinder msg: 1730868168.50 sec; seqNo:366; #beams:4 2024-11-06T04:42:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.47 , 39.15 , 39.15 , 43.84 2024-11-06T04:42:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.06 , -0.12 2024-11-06T04:42:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868168.52 epoch sec; seqNo:1117; 4061665.72 north; 594300.25 east; 36.70 lat; -121.94 lon; 50.91 depth; utmZone: 10 2024-11-06T04:42:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868168.09 2024-11-06T04:42:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:48Z- waiting for 1730868172.09, time is 1730868168.52 2024-11-06T04:42:48Z- AHRS_M2 msg: 1730868168.89 epoch sec; seqNo:1026 2024-11-06T04:42:48Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 1.54 psi 2024-11-06T04:42:48Z- handleDepth()- Depth_Keller msg: 1730868168.90 epoch sec; seqNo:1118; depth 50.93 2024-11-06T04:42:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868168.91 epoch sec; seqNo:1118; 4061665.72 north; 594300.25 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T04:42:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868168.09 2024-11-06T04:42:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:49Z- waiting for 1730868172.09, time is 1730868168.91 2024-11-06T04:42:49Z- AHRS_M2 msg: 1730868169.29 epoch sec; seqNo:1027 2024-11-06T04:42:49Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 1.54 psi 2024-11-06T04:42:49Z- handleDepth()- Depth_Keller msg: 1730868169.29 epoch sec; seqNo:1119; depth 50.95 2024-11-06T04:42:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868169.31 epoch sec; seqNo:1119; 4061665.72 north; 594300.25 east; 36.70 lat; -121.94 lon; 50.95 depth; utmZone: 10 2024-11-06T04:42:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868168.09 2024-11-06T04:42:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:49Z- waiting for 1730868172.09, time is 1730868169.31 2024-11-06T04:42:49Z- AHRS_M2 msg: 1730868169.69 epoch sec; seqNo:1028 2024-11-06T04:42:49Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 1.53 psi 2024-11-06T04:42:49Z- handleDepth()- Depth_Keller msg: 1730868169.70 epoch sec; seqNo:1120; depth 50.95 2024-11-06T04:42:49Z- handleDVL() - DVL msg epochMillisec: 1730868169.70 2024-11-06T04:42:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:42:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:42:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:42:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:42:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:42:49Z- handleDvl() - RDI_Pathfinder msg: 1730868169.70 sec; seqNo:367; #beams:0 2024-11-06T04:42:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:42:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.06 , -0.12 2024-11-06T04:42:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868169.71 epoch sec; seqNo:1120; 4061665.73 north; 594301.47 east; 36.70 lat; -121.94 lon; 50.95 depth; utmZone: 10 2024-11-06T04:42:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868168.09 2024-11-06T04:42:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:49Z- waiting for 1730868172.09, time is 1730868169.71 2024-11-06T04:42:50Z- AHRS_M2 msg: 1730868170.10 epoch sec; seqNo:1029 2024-11-06T04:42:50Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 1.53 psi 2024-11-06T04:42:50Z- handleDepth()- Depth_Keller msg: 1730868170.10 epoch sec; seqNo:1121; depth 50.96 2024-11-06T04:42:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868170.11 epoch sec; seqNo:1121; 4061665.73 north; 594301.47 east; 36.70 lat; -121.94 lon; 50.96 depth; utmZone: 10 2024-11-06T04:42:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868168.09 2024-11-06T04:42:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:50Z- waiting for 1730868172.09, time is 1730868170.11 2024-11-06T04:42:50Z- AHRS_M2 msg: 1730868170.50 epoch sec; seqNo:1030 2024-11-06T04:42:50Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.53 psi 2024-11-06T04:42:50Z- handleDepth()- Depth_Keller msg: 1730868170.52 epoch sec; seqNo:1122; depth 50.98 2024-11-06T04:42:50Z- handleDVL() - DVL msg epochMillisec: 1730868170.54 2024-11-06T04:42:50Z- handleDvl() - RDI_Pathfinder msg: 1730868170.54 sec; seqNo:368; #beams:4 2024-11-06T04:42:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.27 , 38.74 , 39.35 , 43.23 2024-11-06T04:42:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.10 , -0.08 2024-11-06T04:42:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868170.55 epoch sec; seqNo:1122; 4061665.73 north; 594301.47 east; 36.70 lat; -121.94 lon; 50.98 depth; utmZone: 10 2024-11-06T04:42:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868168.09 2024-11-06T04:42:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:50Z- waiting for 1730868172.09, time is 1730868170.55 2024-11-06T04:42:50Z- handleDepth()- Depth_Keller msg: 1730868170.93 epoch sec; seqNo:1123; depth 51.00 2024-11-06T04:42:50Z- handleDVL() - DVL msg epochMillisec: 1730868170.93 2024-11-06T04:42:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:42:50Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:42:50Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:42:50Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:42:50Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:42:50Z- handleDvl() - RDI_Pathfinder msg: 1730868170.93 sec; seqNo:369; #beams:0 2024-11-06T04:42:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:42:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.10 , -0.08 2024-11-06T04:42:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868170.95 epoch sec; seqNo:1123; 4061665.75 north; 594302.69 east; 36.70 lat; -121.94 lon; 51.00 depth; utmZone: 10 2024-11-06T04:42:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868168.09 2024-11-06T04:42:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:51Z- waiting for 1730868172.09, time is 1730868170.95 2024-11-06T04:42:51Z- AHRS_M2 msg: 1730868171.31 epoch sec; seqNo:1031 2024-11-06T04:42:51Z- AHRS_M2 msg: 0.14 phi; -0.07 theta; 1.53 psi 2024-11-06T04:42:51Z- handleDepth()- Depth_Keller msg: 1730868171.32 epoch sec; seqNo:1124; depth 51.01 2024-11-06T04:42:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868171.33 epoch sec; seqNo:1124; 4061666.12 north; 594302.69 east; 36.70 lat; -121.94 lon; 51.01 depth; utmZone: 10 2024-11-06T04:42:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868168.09 2024-11-06T04:42:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:51Z- waiting for 1730868172.09, time is 1730868171.33 2024-11-06T04:42:51Z- AHRS_M2 msg: 1730868171.74 epoch sec; seqNo:1032 2024-11-06T04:42:51Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.53 psi 2024-11-06T04:42:51Z- handleDepth()- Depth_Keller msg: 1730868171.74 epoch sec; seqNo:1125; depth 51.03 2024-11-06T04:42:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868171.79 epoch sec; seqNo:1125; 4061666.12 north; 594302.69 east; 36.70 lat; -121.94 lon; 51.03 depth; utmZone: 10 2024-11-06T04:42:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868168.09 2024-11-06T04:42:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:51Z- waiting for 1730868172.09, time is 1730868171.79 2024-11-06T04:42:52Z- AHRS_M2 msg: 1730868172.11 epoch sec; seqNo:1033 2024-11-06T04:42:52Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.52 psi 2024-11-06T04:42:52Z- handleDepth()- Depth_Keller msg: 1730868172.12 epoch sec; seqNo:1126; depth 51.05 2024-11-06T04:42:52Z- handleDVL() - DVL msg epochMillisec: 1730868172.12 2024-11-06T04:42:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:42:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:42:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:42:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:42:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:42:52Z- handleDvl() - RDI_Pathfinder msg: 1730868172.12 sec; seqNo:370; #beams:0 2024-11-06T04:42:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:42:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.10 , -0.08 2024-11-06T04:42:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868172.13 epoch sec; seqNo:1126; 4061666.12 north; 594302.69 east; 36.70 lat; -121.94 lon; 51.05 depth; utmZone: 10 2024-11-06T04:42:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868168.09 2024-11-06T04:42:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:52Z- waiting for 1730868172.09, time is 1730868172.13 2024-11-06T04:42:52Z- LcmTrn::updateTrn() >>>> pose time 1730868172.13, meas time 1730868172.12 <<<< 2024-11-06T04:42:52Z- LcmTrn::updateTrn() equating pose 1730868172.12 and meas times 1730868172.12 2024-11-06T04:42:52Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868172.12 2024-11-06T04:42:52Z- TerrainNav::motionUpdate() - currEstimate = (4061666, 594302.69, 51.05) 2024-11-06T04:42:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:42:52Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:42:52Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:42:52Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:42:52Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:42:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:42:52Z- variances([0], [2], and psi_berg) = 0.174807 0.176037 0.000000 2024-11-06T04:42:52Z- publishEstimatesDecoder() - at 1730868172.12 sec 2024-11-06T04:42:52Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:42:52Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061609.43 594307.32 57.02 2024-11-06T04:42:52Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061610.05 594306.76 56.71 lat/lon: 36.70 -121.94 2024-11-06T04:42:52Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.17 0.18 1.03 0.00 2024-11-06T04:42:52Z- AHRS_M2 msg: 1730868172.52 epoch sec; seqNo:1034 2024-11-06T04:42:52Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.52 psi 2024-11-06T04:42:52Z- handleDepth()- Depth_Keller msg: 1730868172.53 epoch sec; seqNo:1127; depth 51.06 2024-11-06T04:42:52Z- handleDVL() - DVL msg epochMillisec: 1730868172.55 2024-11-06T04:42:52Z- handleDvl() - RDI_Pathfinder msg: 1730868172.55 sec; seqNo:371; #beams:4 2024-11-06T04:42:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.86 , 38.88 , 39.44 , 42.80 2024-11-06T04:42:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.10 , 0.05 2024-11-06T04:42:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868172.57 epoch sec; seqNo:1127; 4061666.12 north; 594302.69 east; 36.70 lat; -121.94 lon; 51.06 depth; utmZone: 10 2024-11-06T04:42:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868172.12 2024-11-06T04:42:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:52Z- waiting for 1730868176.12, time is 1730868172.57 2024-11-06T04:42:52Z- AHRS_M2 msg: 1730868172.92 epoch sec; seqNo:1035 2024-11-06T04:42:52Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.52 psi 2024-11-06T04:42:52Z- handleDepth()- Depth_Keller msg: 1730868172.93 epoch sec; seqNo:1128; depth 51.10 2024-11-06T04:42:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868172.96 epoch sec; seqNo:1128; 4061666.14 north; 594303.91 east; 36.70 lat; -121.94 lon; 51.10 depth; utmZone: 10 2024-11-06T04:42:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868172.12 2024-11-06T04:42:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:53Z- waiting for 1730868176.12, time is 1730868172.96 2024-11-06T04:42:53Z- AHRS_M2 msg: 1730868173.33 epoch sec; seqNo:1036 2024-11-06T04:42:53Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.52 psi 2024-11-06T04:42:53Z- handleDepth()- Depth_Keller msg: 1730868173.34 epoch sec; seqNo:1129; depth 51.12 2024-11-06T04:42:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868173.35 epoch sec; seqNo:1129; 4061666.14 north; 594303.91 east; 36.70 lat; -121.94 lon; 51.12 depth; utmZone: 10 2024-11-06T04:42:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868172.12 2024-11-06T04:42:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:53Z- waiting for 1730868176.12, time is 1730868173.35 2024-11-06T04:42:53Z- AHRS_M2 msg: 1730868173.73 epoch sec; seqNo:1037 2024-11-06T04:42:53Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.52 psi 2024-11-06T04:42:53Z- handleDepth()- Depth_Keller msg: 1730868173.74 epoch sec; seqNo:1130; depth 51.13 2024-11-06T04:42:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868173.75 epoch sec; seqNo:1130; 4061666.14 north; 594303.91 east; 36.70 lat; -121.94 lon; 51.13 depth; utmZone: 10 2024-11-06T04:42:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868172.12 2024-11-06T04:42:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:53Z- waiting for 1730868176.12, time is 1730868173.75 2024-11-06T04:42:54Z- AHRS_M2 msg: 1730868174.13 epoch sec; seqNo:1038 2024-11-06T04:42:54Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.52 psi 2024-11-06T04:42:54Z- handleDepth()- Depth_Keller msg: 1730868174.15 epoch sec; seqNo:1131; depth 51.16 2024-11-06T04:42:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868174.17 epoch sec; seqNo:1131; 4061666.52 north; 594303.90 east; 36.70 lat; -121.94 lon; 51.16 depth; utmZone: 10 2024-11-06T04:42:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868172.12 2024-11-06T04:42:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:54Z- waiting for 1730868176.12, time is 1730868174.17 2024-11-06T04:42:54Z- AHRS_M2 msg: 1730868174.54 epoch sec; seqNo:1039 2024-11-06T04:42:54Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.52 psi 2024-11-06T04:42:54Z- handleDepth()- Depth_Keller msg: 1730868174.54 epoch sec; seqNo:1132; depth 51.17 2024-11-06T04:42:54Z- handleDVL() - DVL msg epochMillisec: 1730868174.59 2024-11-06T04:42:54Z- handleDvl() - RDI_Pathfinder msg: 1730868174.59 sec; seqNo:372; #beams:4 2024-11-06T04:42:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.05 , 39.06 , 39.25 , 43.18 2024-11-06T04:42:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , 0.14 2024-11-06T04:42:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868174.60 epoch sec; seqNo:1132; 4061666.53 north; 594305.12 east; 36.70 lat; -121.94 lon; 51.17 depth; utmZone: 10 2024-11-06T04:42:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868172.12 2024-11-06T04:42:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:54Z- waiting for 1730868176.12, time is 1730868174.60 2024-11-06T04:42:54Z- AHRS_M2 msg: 1730868174.95 epoch sec; seqNo:1040 2024-11-06T04:42:54Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.52 psi 2024-11-06T04:42:54Z- handleDepth()- Depth_Keller msg: 1730868174.95 epoch sec; seqNo:1133; depth 51.21 2024-11-06T04:42:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868175.01 epoch sec; seqNo:1133; 4061666.53 north; 594305.12 east; 36.70 lat; -121.94 lon; 51.21 depth; utmZone: 10 2024-11-06T04:42:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868172.12 2024-11-06T04:42:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:55Z- waiting for 1730868176.12, time is 1730868175.01 2024-11-06T04:42:55Z- handleDepth()- Depth_Keller msg: 1730868175.37 epoch sec; seqNo:1134; depth 51.23 2024-11-06T04:42:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868175.38 epoch sec; seqNo:1134; 4061666.53 north; 594305.12 east; 36.70 lat; -121.94 lon; 51.23 depth; utmZone: 10 2024-11-06T04:42:55Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:42:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868172.12 2024-11-06T04:42:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:55Z- waiting for 1730868176.12, time is 1730868175.38 2024-11-06T04:42:55Z- AHRS_M2 msg: 1730868175.75 epoch sec; seqNo:1041 2024-11-06T04:42:55Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.52 psi 2024-11-06T04:42:55Z- handleDepth()- Depth_Keller msg: 1730868175.76 epoch sec; seqNo:1135; depth 51.25 2024-11-06T04:42:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868175.78 epoch sec; seqNo:1135; 4061666.53 north; 594305.12 east; 36.70 lat; -121.94 lon; 51.25 depth; utmZone: 10 2024-11-06T04:42:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868172.12 2024-11-06T04:42:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:55Z- waiting for 1730868176.12, time is 1730868175.78 2024-11-06T04:42:56Z- AHRS_M2 msg: 1730868176.16 epoch sec; seqNo:1042 2024-11-06T04:42:56Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.52 psi 2024-11-06T04:42:56Z- handleDepth()- Depth_Keller msg: 1730868176.17 epoch sec; seqNo:1136; depth 51.27 2024-11-06T04:42:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868176.19 epoch sec; seqNo:1136; 4061666.54 north; 594306.34 east; 36.70 lat; -121.94 lon; 51.27 depth; utmZone: 10 2024-11-06T04:42:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868172.12 2024-11-06T04:42:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:56Z- waiting for 1730868176.12, time is 1730868176.19 2024-11-06T04:42:56Z- LcmTrn::updateTrn() >>>> pose time 1730868176.19, meas time 1730868174.59 <<<< 2024-11-06T04:42:56Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868176.19 2024-11-06T04:42:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868174.59 sec, ping # 0; waiting for INS data more recent than 1730868172.12... 2024-11-06T04:42:56Z- TerrainNav::motionUpdate() - currEstimate = (4061667, 594306.34, 51.27) 2024-11-06T04:42:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:42:56Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:42:56Z- TNavPF::Projecting Measurements 2024-11-06T04:42:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:42:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:42:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:42:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:42:56Z- TNavFilter::4 good beams. 2024-11-06T04:42:56Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:42:56Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:42:56Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:42:56Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:42:56Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:42:56Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:42:56Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:42:56Z- TerrainMapOctree: (vehN, vehE ) = (4061666.38, 594304.90). 2024-11-06T04:42:56Z- TerrainMapOctree: Min Distance = 100.03. 2024-11-06T04:42:56Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:42:56Z- Forcing Subcloud Comparison 2024-11-06T04:42:56Z- Weighting particles with subcloud comparison 2024-11-06T04:42:56Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:56Z- meanExpectedMeasDiff: 0.818415 delta_rms_squared: 2.077159 alpha: 0.990463 2024-11-06T04:42:56Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:56Z- meanExpectedMeasDiff: 1.268328 delta_rms_squared: 1.229825 alpha: 0.983998 2024-11-06T04:42:56Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:56Z- meanExpectedMeasDiff: 0.827990 delta_rms_squared: 1.656694 alpha: 0.988072 2024-11-06T04:42:56Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:42:56Z- meanExpectedMeasDiff: 0.007098 delta_rms_squared: 2.401410 alpha: 0.991740 2024-11-06T04:42:56Z- Subcloud NIS: 0.169690 2024-11-06T04:42:56Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:42:56Z- TNavPF:: sumWeights = 0.496075 2024-11-06T04:42:56Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:42:56Z- TNavPF::First Term of Map Covariance Matrix is 0.42 2024-11-06T04:42:56Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T04:42:56Z- TerrainNav::Resampling particle filter... 2024-11-06T04:42:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868174.59 sec. 2024-11-06T04:42:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:42:56Z- variances([0], [2], and psi_berg) = 0.175076 0.174813 0.000000 2024-11-06T04:42:56Z- publishEstimatesDecoder() - at 1730868176.19 sec 2024-11-06T04:42:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:42:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061610.73 594310.72 56.02 2024-11-06T04:42:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061610.51 594310.44 56.57 lat/lon: 36.70 -121.94 2024-11-06T04:42:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.17 0.75 0.00 2024-11-06T04:42:56Z- AHRS_M2 msg: 1730868176.57 epoch sec; seqNo:1043 2024-11-06T04:42:56Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.52 psi 2024-11-06T04:42:56Z- handleDepth()- Depth_Keller msg: 1730868176.57 epoch sec; seqNo:1137; depth 51.27 2024-11-06T04:42:56Z- handleDVL() - DVL msg epochMillisec: 1730868176.60 2024-11-06T04:42:56Z- handleDvl() - RDI_Pathfinder msg: 1730868176.60 sec; seqNo:373; #beams:4 2024-11-06T04:42:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.86 , 39.06 , 39.63 , 42.99 2024-11-06T04:42:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , 0.11 2024-11-06T04:42:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868176.63 epoch sec; seqNo:1137; 4061666.54 north; 594306.34 east; 36.70 lat; -121.94 lon; 51.27 depth; utmZone: 10 2024-11-06T04:42:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868176.19 2024-11-06T04:42:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:56Z- waiting for 1730868180.19, time is 1730868176.63 2024-11-06T04:42:56Z- AHRS_M2 msg: 1730868176.97 epoch sec; seqNo:1044 2024-11-06T04:42:56Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.52 psi 2024-11-06T04:42:56Z- handleDepth()- Depth_Keller msg: 1730868176.98 epoch sec; seqNo:1138; depth 51.28 2024-11-06T04:42:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868176.99 epoch sec; seqNo:1138; 4061666.54 north; 594306.34 east; 36.70 lat; -121.94 lon; 51.28 depth; utmZone: 10 2024-11-06T04:42:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868176.19 2024-11-06T04:42:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:57Z- waiting for 1730868180.19, time is 1730868176.99 2024-11-06T04:42:57Z- AHRS_M2 msg: 1730868177.37 epoch sec; seqNo:1045 2024-11-06T04:42:57Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.53 psi 2024-11-06T04:42:57Z- handleDepth()- Depth_Keller msg: 1730868177.37 epoch sec; seqNo:1139; depth 51.31 2024-11-06T04:42:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868177.38 epoch sec; seqNo:1139; 4061666.56 north; 594307.56 east; 36.70 lat; -121.94 lon; 51.31 depth; utmZone: 10 2024-11-06T04:42:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868176.19 2024-11-06T04:42:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:57Z- waiting for 1730868180.19, time is 1730868177.38 2024-11-06T04:42:57Z- AHRS_M2 msg: 1730868177.77 epoch sec; seqNo:1046 2024-11-06T04:42:57Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.53 psi 2024-11-06T04:42:57Z- handleDepth()- Depth_Keller msg: 1730868177.78 epoch sec; seqNo:1140; depth 51.30 2024-11-06T04:42:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868177.79 epoch sec; seqNo:1140; 4061666.56 north; 594307.56 east; 36.70 lat; -121.94 lon; 51.30 depth; utmZone: 10 2024-11-06T04:42:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868176.19 2024-11-06T04:42:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:57Z- waiting for 1730868180.19, time is 1730868177.79 2024-11-06T04:42:58Z- AHRS_M2 msg: 1730868178.17 epoch sec; seqNo:1047 2024-11-06T04:42:58Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.53 psi 2024-11-06T04:42:58Z- handleDepth()- Depth_Keller msg: 1730868178.19 epoch sec; seqNo:1141; depth 51.31 2024-11-06T04:42:58Z- handleDVL() - DVL msg epochMillisec: 1730868178.20 2024-11-06T04:42:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:42:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:42:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:42:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:42:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:42:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:42:58Z- handleDvl() - RDI_Pathfinder msg: 1730868178.20 sec; seqNo:374; #beams:0 2024-11-06T04:42:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:42:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , 0.11 2024-11-06T04:42:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868178.21 epoch sec; seqNo:1141; 4061666.56 north; 594307.56 east; 36.70 lat; -121.94 lon; 51.31 depth; utmZone: 10 2024-11-06T04:42:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868176.19 2024-11-06T04:42:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:58Z- waiting for 1730868180.19, time is 1730868178.21 2024-11-06T04:42:58Z- AHRS_M2 msg: 1730868178.58 epoch sec; seqNo:1048 2024-11-06T04:42:58Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.53 psi 2024-11-06T04:42:58Z- handleDepth()- Depth_Keller msg: 1730868178.58 epoch sec; seqNo:1142; depth 51.30 2024-11-06T04:42:58Z- handleDVL() - DVL msg epochMillisec: 1730868178.61 2024-11-06T04:42:58Z- handleDvl() - RDI_Pathfinder msg: 1730868178.61 sec; seqNo:375; #beams:4 2024-11-06T04:42:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.86 , 38.88 , 39.63 , 42.80 2024-11-06T04:42:58Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , -0.02 , -0.01 2024-11-06T04:42:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868178.62 epoch sec; seqNo:1142; 4061666.95 north; 594308.78 east; 36.70 lat; -121.94 lon; 51.30 depth; utmZone: 10 2024-11-06T04:42:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:42:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868176.19 2024-11-06T04:42:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:58Z- waiting for 1730868180.19, time is 1730868178.62 2024-11-06T04:42:58Z- AHRS_M2 msg: 1730868178.99 epoch sec; seqNo:1049 2024-11-06T04:42:58Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.54 psi 2024-11-06T04:42:58Z- handleDepth()- Depth_Keller msg: 1730868178.99 epoch sec; seqNo:1143; depth 51.31 2024-11-06T04:42:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868179.01 epoch sec; seqNo:1143; 4061666.95 north; 594308.78 east; 36.70 lat; -121.94 lon; 51.31 depth; utmZone: 10 2024-11-06T04:42:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868176.19 2024-11-06T04:42:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:59Z- waiting for 1730868180.19, time is 1730868179.01 2024-11-06T04:42:59Z- AHRS_M2 msg: 1730868179.39 epoch sec; seqNo:1050 2024-11-06T04:42:59Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.54 psi 2024-11-06T04:42:59Z- handleDepth()- Depth_Keller msg: 1730868179.40 epoch sec; seqNo:1144; depth 51.31 2024-11-06T04:42:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868179.41 epoch sec; seqNo:1144; 4061666.95 north; 594308.78 east; 36.70 lat; -121.94 lon; 51.31 depth; utmZone: 10 2024-11-06T04:42:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868176.19 2024-11-06T04:42:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:59Z- waiting for 1730868180.19, time is 1730868179.41 2024-11-06T04:42:59Z- AHRS_M2 msg: 1730868179.79 epoch sec; seqNo:1051 2024-11-06T04:42:59Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.54 psi 2024-11-06T04:42:59Z- handleDepth()- Depth_Keller msg: 1730868179.80 epoch sec; seqNo:1145; depth 51.30 2024-11-06T04:42:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868179.81 epoch sec; seqNo:1145; 4061666.96 north; 594310.00 east; 36.70 lat; -121.94 lon; 51.30 depth; utmZone: 10 2024-11-06T04:42:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:42:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868176.19 2024-11-06T04:42:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:42:59Z- waiting for 1730868180.19, time is 1730868179.81 2024-11-06T04:43:00Z- AHRS_M2 msg: 1730868180.19 epoch sec; seqNo:1052 2024-11-06T04:43:00Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.54 psi 2024-11-06T04:43:00Z- handleDepth()- Depth_Keller msg: 1730868180.21 epoch sec; seqNo:1146; depth 51.29 2024-11-06T04:43:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868180.22 epoch sec; seqNo:1146; 4061666.96 north; 594310.00 east; 36.70 lat; -121.94 lon; 51.29 depth; utmZone: 10 2024-11-06T04:43:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868176.19 2024-11-06T04:43:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:00Z- waiting for 1730868180.19, time is 1730868180.22 2024-11-06T04:43:00Z- LcmTrn::updateTrn() >>>> pose time 1730868180.22, meas time 1730868178.61 <<<< 2024-11-06T04:43:00Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868180.22 2024-11-06T04:43:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868178.61 sec, ping # 0; waiting for INS data more recent than 1730868176.19... 2024-11-06T04:43:00Z- TerrainNav::motionUpdate() - currEstimate = (4061667, 594310.00, 51.29) 2024-11-06T04:43:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:43:00Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:43:00Z- TNavPF::Projecting Measurements 2024-11-06T04:43:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:43:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:43:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:43:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:43:00Z- TNavFilter::4 good beams. 2024-11-06T04:43:00Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:43:00Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:43:00Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:43:00Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:43:00Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:43:00Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:43:00Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:43:00Z- TerrainMapOctree: (vehN, vehE ) = (4061666.80, 594308.54). 2024-11-06T04:43:00Z- TerrainMapOctree: Min Distance = 101.73. 2024-11-06T04:43:00Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:43:00Z- Forcing Subcloud Comparison 2024-11-06T04:43:00Z- Weighting particles with subcloud comparison 2024-11-06T04:43:00Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:00Z- meanExpectedMeasDiff: 1.415631 delta_rms_squared: 1.599025 alpha: 0.987647 2024-11-06T04:43:00Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:00Z- meanExpectedMeasDiff: -0.008541 delta_rms_squared: 1.159507 alpha: 0.983044 2024-11-06T04:43:00Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:00Z- meanExpectedMeasDiff: 0.445288 delta_rms_squared: 1.308463 alpha: 0.984945 2024-11-06T04:43:00Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:00Z- meanExpectedMeasDiff: -0.020448 delta_rms_squared: 1.402245 alpha: 0.985938 2024-11-06T04:43:00Z- Subcloud NIS: 0.155445 2024-11-06T04:43:00Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:43:00Z- TNavPF:: sumWeights = 0.566906 2024-11-06T04:43:00Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:43:00Z- TNavPF::First Term of Map Covariance Matrix is 0.56 2024-11-06T04:43:00Z- TNavPF::Calculated NIS Value : 0.14 numBeams normalized NIS: 0.03 2024-11-06T04:43:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868178.61 sec. 2024-11-06T04:43:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:43:00Z- variances([0], [2], and psi_berg) = 0.175056 0.178230 0.000000 2024-11-06T04:43:00Z- publishEstimatesDecoder() - at 1730868180.22 sec 2024-11-06T04:43:00Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:43:00Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061609.89 594313.21 56.72 2024-11-06T04:43:00Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061610.93 594314.09 56.36 lat/lon: 36.70 -121.94 2024-11-06T04:43:00Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.18 0.72 0.00 2024-11-06T04:43:00Z- AHRS_M2 msg: 1730868180.60 epoch sec; seqNo:1053 2024-11-06T04:43:00Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.55 psi 2024-11-06T04:43:00Z- handleDepth()- Depth_Keller msg: 1730868180.61 epoch sec; seqNo:1147; depth 51.27 2024-11-06T04:43:00Z- handleDVL() - DVL msg epochMillisec: 1730868180.64 2024-11-06T04:43:00Z- handleDvl() - RDI_Pathfinder msg: 1730868180.64 sec; seqNo:376; #beams:4 2024-11-06T04:43:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.67 , 38.32 , 39.63 , 42.24 2024-11-06T04:43:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , -0.11 2024-11-06T04:43:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868180.65 epoch sec; seqNo:1147; 4061666.96 north; 594310.00 east; 36.70 lat; -121.94 lon; 51.27 depth; utmZone: 10 2024-11-06T04:43:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868180.22 2024-11-06T04:43:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:00Z- waiting for 1730868184.22, time is 1730868180.65 2024-11-06T04:43:01Z- handleDepth()- Depth_Keller msg: 1730868181.03 epoch sec; seqNo:1148; depth 51.27 2024-11-06T04:43:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868181.04 epoch sec; seqNo:1148; 4061666.96 north; 594310.00 east; 36.70 lat; -121.94 lon; 51.27 depth; utmZone: 10 2024-11-06T04:43:01Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:43:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868180.22 2024-11-06T04:43:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:01Z- waiting for 1730868184.22, time is 1730868181.04 2024-11-06T04:43:01Z- AHRS_M2 msg: 1730868181.41 epoch sec; seqNo:1054 2024-11-06T04:43:01Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.55 psi 2024-11-06T04:43:01Z- handleDepth()- Depth_Keller msg: 1730868181.43 epoch sec; seqNo:1149; depth 51.25 2024-11-06T04:43:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868181.44 epoch sec; seqNo:1149; 4061666.98 north; 594311.22 east; 36.70 lat; -121.94 lon; 51.25 depth; utmZone: 10 2024-11-06T04:43:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868180.22 2024-11-06T04:43:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:01Z- waiting for 1730868184.22, time is 1730868181.44 2024-11-06T04:43:01Z- AHRS_M2 msg: 1730868181.81 epoch sec; seqNo:1055 2024-11-06T04:43:01Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.55 psi 2024-11-06T04:43:01Z- handleDepth()- Depth_Keller msg: 1730868181.82 epoch sec; seqNo:1150; depth 51.25 2024-11-06T04:43:01Z- handleDVL() - DVL msg epochMillisec: 1730868181.82 2024-11-06T04:43:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:01Z- handleDvl() - RDI_Pathfinder msg: 1730868181.82 sec; seqNo:377; #beams:0 2024-11-06T04:43:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , -0.11 2024-11-06T04:43:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868181.84 epoch sec; seqNo:1150; 4061666.98 north; 594311.22 east; 36.70 lat; -121.94 lon; 51.25 depth; utmZone: 10 2024-11-06T04:43:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868180.22 2024-11-06T04:43:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:01Z- waiting for 1730868184.22, time is 1730868181.84 2024-11-06T04:43:02Z- AHRS_M2 msg: 1730868182.23 epoch sec; seqNo:1056 2024-11-06T04:43:02Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.55 psi 2024-11-06T04:43:02Z- handleDepth()- Depth_Keller msg: 1730868182.24 epoch sec; seqNo:1151; depth 51.24 2024-11-06T04:43:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868182.25 epoch sec; seqNo:1151; 4061666.98 north; 594311.22 east; 36.70 lat; -121.94 lon; 51.24 depth; utmZone: 10 2024-11-06T04:43:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868180.22 2024-11-06T04:43:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:02Z- waiting for 1730868184.22, time is 1730868182.25 2024-11-06T04:43:02Z- AHRS_M2 msg: 1730868182.62 epoch sec; seqNo:1057 2024-11-06T04:43:02Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.55 psi 2024-11-06T04:43:02Z- handleDepth()- Depth_Keller msg: 1730868182.63 epoch sec; seqNo:1152; depth 51.23 2024-11-06T04:43:02Z- handleDVL() - DVL msg epochMillisec: 1730868182.65 2024-11-06T04:43:02Z- handleDvl() - RDI_Pathfinder msg: 1730868182.65 sec; seqNo:378; #beams:4 2024-11-06T04:43:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.49 , 37.94 , 39.81 , 41.68 2024-11-06T04:43:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.05 , -0.13 2024-11-06T04:43:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868182.67 epoch sec; seqNo:1152; 4061666.99 north; 594312.44 east; 36.70 lat; -121.94 lon; 51.23 depth; utmZone: 10 2024-11-06T04:43:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868180.22 2024-11-06T04:43:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:02Z- waiting for 1730868184.22, time is 1730868182.67 2024-11-06T04:43:03Z- AHRS_M2 msg: 1730868183.03 epoch sec; seqNo:1058 2024-11-06T04:43:03Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.54 psi 2024-11-06T04:43:03Z- handleDepth()- Depth_Keller msg: 1730868183.06 epoch sec; seqNo:1153; depth 51.22 2024-11-06T04:43:03Z- handleDVL() - DVL msg epochMillisec: 1730868183.06 2024-11-06T04:43:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:03Z- handleDvl() - RDI_Pathfinder msg: 1730868183.06 sec; seqNo:379; #beams:0 2024-11-06T04:43:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.05 , -0.13 2024-11-06T04:43:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868183.08 epoch sec; seqNo:1153; 4061666.99 north; 594312.44 east; 36.70 lat; -121.94 lon; 51.22 depth; utmZone: 10 2024-11-06T04:43:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868180.22 2024-11-06T04:43:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:03Z- waiting for 1730868184.22, time is 1730868183.08 2024-11-06T04:43:03Z- AHRS_M2 msg: 1730868183.43 epoch sec; seqNo:1059 2024-11-06T04:43:03Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.54 psi 2024-11-06T04:43:03Z- handleDepth()- Depth_Keller msg: 1730868183.44 epoch sec; seqNo:1154; depth 51.19 2024-11-06T04:43:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868183.45 epoch sec; seqNo:1154; 4061666.99 north; 594312.44 east; 36.70 lat; -121.94 lon; 51.19 depth; utmZone: 10 2024-11-06T04:43:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868180.22 2024-11-06T04:43:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:03Z- waiting for 1730868184.22, time is 1730868183.45 2024-11-06T04:43:03Z- AHRS_M2 msg: 1730868183.84 epoch sec; seqNo:1060 2024-11-06T04:43:03Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.54 psi 2024-11-06T04:43:03Z- handleDepth()- Depth_Keller msg: 1730868183.85 epoch sec; seqNo:1155; depth 51.19 2024-11-06T04:43:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868183.86 epoch sec; seqNo:1155; 4061666.99 north; 594312.44 east; 36.70 lat; -121.94 lon; 51.19 depth; utmZone: 10 2024-11-06T04:43:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868180.22 2024-11-06T04:43:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:04Z- waiting for 1730868184.22, time is 1730868183.86 2024-11-06T04:43:04Z- AHRS_M2 msg: 1730868184.25 epoch sec; seqNo:1061 2024-11-06T04:43:04Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.53 psi 2024-11-06T04:43:04Z- handleDepth()- Depth_Keller msg: 1730868184.26 epoch sec; seqNo:1156; depth 51.17 2024-11-06T04:43:04Z- handleDVL() - DVL msg epochMillisec: 1730868184.26 2024-11-06T04:43:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:04Z- handleDvl() - RDI_Pathfinder msg: 1730868184.26 sec; seqNo:380; #beams:0 2024-11-06T04:43:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.05 , -0.13 2024-11-06T04:43:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868184.27 epoch sec; seqNo:1156; 4061667.00 north; 594313.66 east; 36.70 lat; -121.94 lon; 51.17 depth; utmZone: 10 2024-11-06T04:43:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868180.22 2024-11-06T04:43:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:04Z- waiting for 1730868184.22, time is 1730868184.27 2024-11-06T04:43:04Z- LcmTrn::updateTrn() >>>> pose time 1730868184.27, meas time 1730868184.26 <<<< 2024-11-06T04:43:04Z- LcmTrn::updateTrn() equating pose 1730868184.26 and meas times 1730868184.26 2024-11-06T04:43:04Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868184.26 2024-11-06T04:43:04Z- TerrainNav::motionUpdate() - currEstimate = (4061667, 594313.66, 51.17) 2024-11-06T04:43:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:43:04Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:43:04Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:43:04Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:43:04Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:43:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:43:04Z- variances([0], [2], and psi_berg) = 0.176768 0.180305 0.000000 2024-11-06T04:43:04Z- publishEstimatesDecoder() - at 1730868184.26 sec 2024-11-06T04:43:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:43:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061609.98 594316.90 56.54 2024-11-06T04:43:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061610.97 594317.75 56.23 lat/lon: 36.70 -121.94 2024-11-06T04:43:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.18 0.97 0.00 2024-11-06T04:43:04Z- AHRS_M2 msg: 1730868184.64 epoch sec; seqNo:1062 2024-11-06T04:43:04Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.53 psi 2024-11-06T04:43:04Z- handleDepth()- Depth_Keller msg: 1730868184.65 epoch sec; seqNo:1157; depth 51.17 2024-11-06T04:43:04Z- handleDVL() - DVL msg epochMillisec: 1730868184.68 2024-11-06T04:43:04Z- handleDvl() - RDI_Pathfinder msg: 1730868184.68 sec; seqNo:381; #beams:4 2024-11-06T04:43:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.74 , 38.13 , 39.25 , 41.12 2024-11-06T04:43:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.68 , -0.08 , -0.04 2024-11-06T04:43:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868184.70 epoch sec; seqNo:1157; 4061667.00 north; 594313.66 east; 36.70 lat; -121.94 lon; 51.17 depth; utmZone: 10 2024-11-06T04:43:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868184.26 2024-11-06T04:43:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:04Z- waiting for 1730868188.26, time is 1730868184.70 2024-11-06T04:43:05Z- AHRS_M2 msg: 1730868185.05 epoch sec; seqNo:1063 2024-11-06T04:43:05Z- AHRS_M2 msg: 0.12 phi; -0.00 theta; 1.53 psi 2024-11-06T04:43:05Z- handleDepth()- Depth_Keller msg: 1730868185.06 epoch sec; seqNo:1158; depth 51.17 2024-11-06T04:43:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868185.07 epoch sec; seqNo:1158; 4061667.38 north; 594313.66 east; 36.70 lat; -121.94 lon; 51.17 depth; utmZone: 10 2024-11-06T04:43:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868184.26 2024-11-06T04:43:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:05Z- waiting for 1730868188.26, time is 1730868185.07 2024-11-06T04:43:05Z- AHRS_M2 msg: 1730868185.45 epoch sec; seqNo:1064 2024-11-06T04:43:05Z- AHRS_M2 msg: 0.12 phi; -0.00 theta; 1.52 psi 2024-11-06T04:43:05Z- handleDepth()- Depth_Keller msg: 1730868185.46 epoch sec; seqNo:1159; depth 51.16 2024-11-06T04:43:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868185.48 epoch sec; seqNo:1159; 4061667.38 north; 594313.66 east; 36.70 lat; -121.94 lon; 51.16 depth; utmZone: 10 2024-11-06T04:43:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868184.26 2024-11-06T04:43:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:05Z- waiting for 1730868188.26, time is 1730868185.48 2024-11-06T04:43:05Z- AHRS_M2 msg: 1730868185.86 epoch sec; seqNo:1065 2024-11-06T04:43:05Z- AHRS_M2 msg: 0.12 phi; -0.00 theta; 1.52 psi 2024-11-06T04:43:05Z- handleDepth()- Depth_Keller msg: 1730868185.86 epoch sec; seqNo:1160; depth 51.18 2024-11-06T04:43:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868185.87 epoch sec; seqNo:1160; 4061667.40 north; 594314.88 east; 36.70 lat; -121.94 lon; 51.18 depth; utmZone: 10 2024-11-06T04:43:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868184.26 2024-11-06T04:43:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:06Z- waiting for 1730868188.26, time is 1730868185.87 2024-11-06T04:43:06Z- AHRS_M2 msg: 1730868186.26 epoch sec; seqNo:1066 2024-11-06T04:43:06Z- AHRS_M2 msg: 0.12 phi; -0.00 theta; 1.51 psi 2024-11-06T04:43:06Z- handleDepth()- Depth_Keller msg: 1730868186.27 epoch sec; seqNo:1161; depth 51.16 2024-11-06T04:43:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868186.28 epoch sec; seqNo:1161; 4061667.40 north; 594314.88 east; 36.70 lat; -121.94 lon; 51.16 depth; utmZone: 10 2024-11-06T04:43:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868184.26 2024-11-06T04:43:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:06Z- waiting for 1730868188.26, time is 1730868186.28 2024-11-06T04:43:06Z- handleDepth()- Depth_Keller msg: 1730868186.68 epoch sec; seqNo:1162; depth 51.17 2024-11-06T04:43:06Z- handleDVL() - DVL msg epochMillisec: 1730868186.70 2024-11-06T04:43:06Z- handleDvl() - RDI_Pathfinder msg: 1730868186.70 sec; seqNo:382; #beams:4 2024-11-06T04:43:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.17 , 38.13 , 39.06 , 41.12 2024-11-06T04:43:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.07 , 0.08 2024-11-06T04:43:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868186.73 epoch sec; seqNo:1162; 4061667.40 north; 594314.88 east; 36.70 lat; -121.94 lon; 51.17 depth; utmZone: 10 2024-11-06T04:43:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868184.26 2024-11-06T04:43:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:06Z- waiting for 1730868188.26, time is 1730868186.73 2024-11-06T04:43:07Z- AHRS_M2 msg: 1730868187.07 epoch sec; seqNo:1067 2024-11-06T04:43:07Z- AHRS_M2 msg: 0.11 phi; 0.00 theta; 1.50 psi 2024-11-06T04:43:07Z- handleDepth()- Depth_Keller msg: 1730868187.08 epoch sec; seqNo:1163; depth 51.18 2024-11-06T04:43:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868187.10 epoch sec; seqNo:1163; 4061667.40 north; 594314.88 east; 36.70 lat; -121.94 lon; 51.18 depth; utmZone: 10 2024-11-06T04:43:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868184.26 2024-11-06T04:43:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:07Z- waiting for 1730868188.26, time is 1730868187.10 2024-11-06T04:43:07Z- AHRS_M2 msg: 1730868187.47 epoch sec; seqNo:1068 2024-11-06T04:43:07Z- AHRS_M2 msg: 0.11 phi; 0.01 theta; 1.50 psi 2024-11-06T04:43:07Z- handleDepth()- Depth_Keller msg: 1730868187.48 epoch sec; seqNo:1164; depth 51.17 2024-11-06T04:43:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868187.49 epoch sec; seqNo:1164; 4061667.41 north; 594316.10 east; 36.70 lat; -121.94 lon; 51.17 depth; utmZone: 10 2024-11-06T04:43:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868184.26 2024-11-06T04:43:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:07Z- waiting for 1730868188.26, time is 1730868187.49 2024-11-06T04:43:07Z- AHRS_M2 msg: 1730868187.88 epoch sec; seqNo:1069 2024-11-06T04:43:07Z- AHRS_M2 msg: 0.11 phi; 0.01 theta; 1.49 psi 2024-11-06T04:43:07Z- handleDepth()- Depth_Keller msg: 1730868187.88 epoch sec; seqNo:1165; depth 51.18 2024-11-06T04:43:07Z- handleDVL() - DVL msg epochMillisec: 1730868187.88 2024-11-06T04:43:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:07Z- handleDvl() - RDI_Pathfinder msg: 1730868187.88 sec; seqNo:383; #beams:0 2024-11-06T04:43:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.07 , 0.08 2024-11-06T04:43:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868187.89 epoch sec; seqNo:1165; 4061667.41 north; 594316.10 east; 36.70 lat; -121.94 lon; 51.18 depth; utmZone: 10 2024-11-06T04:43:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868184.26 2024-11-06T04:43:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:08Z- waiting for 1730868188.26, time is 1730868187.89 2024-11-06T04:43:08Z- AHRS_M2 msg: 1730868188.28 epoch sec; seqNo:1070 2024-11-06T04:43:08Z- AHRS_M2 msg: 0.11 phi; 0.01 theta; 1.49 psi 2024-11-06T04:43:08Z- handleDepth()- Depth_Keller msg: 1730868188.29 epoch sec; seqNo:1166; depth 51.19 2024-11-06T04:43:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868188.30 epoch sec; seqNo:1166; 4061667.79 north; 594316.09 east; 36.70 lat; -121.94 lon; 51.19 depth; utmZone: 10 2024-11-06T04:43:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868184.26 2024-11-06T04:43:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:08Z- waiting for 1730868188.26, time is 1730868188.30 2024-11-06T04:43:08Z- LcmTrn::updateTrn() >>>> pose time 1730868188.30, meas time 1730868187.88 <<<< 2024-11-06T04:43:08Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868188.30 2024-11-06T04:43:08Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:43:08Z- TerrainNav::motionUpdate() - currEstimate = (4061668, 594316.09, 51.19) 2024-11-06T04:43:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:43:08Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:43:08Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:43:08Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:43:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:43:08Z- variances([0], [2], and psi_berg) = 0.177923 0.181534 0.000000 2024-11-06T04:43:08Z- publishEstimatesDecoder() - at 1730868188.30 sec 2024-11-06T04:43:08Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:43:08Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061610.70 594319.32 56.23 2024-11-06T04:43:08Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061611.75 594320.19 56.26 lat/lon: 36.70 -121.94 2024-11-06T04:43:08Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.18 1.22 0.00 2024-11-06T04:43:08Z- handleDepth()- Depth_Keller msg: 1730868188.71 epoch sec; seqNo:1167; depth 51.19 2024-11-06T04:43:08Z- handleDVL() - DVL msg epochMillisec: 1730868188.73 2024-11-06T04:43:08Z- handleDvl() - RDI_Pathfinder msg: 1730868188.73 sec; seqNo:384; #beams:4 2024-11-06T04:43:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.99 , 38.50 , 39.25 , 41.12 2024-11-06T04:43:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.13 2024-11-06T04:43:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868188.76 epoch sec; seqNo:1167; 4061667.79 north; 594316.09 east; 36.70 lat; -121.94 lon; 51.19 depth; utmZone: 10 2024-11-06T04:43:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868188.30 2024-11-06T04:43:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:08Z- waiting for 1730868192.30, time is 1730868188.76 2024-11-06T04:43:09Z- AHRS_M2 msg: 1730868189.09 epoch sec; seqNo:1071 2024-11-06T04:43:09Z- AHRS_M2 msg: 0.11 phi; 0.02 theta; 1.49 psi 2024-11-06T04:43:09Z- handleDepth()- Depth_Keller msg: 1730868189.10 epoch sec; seqNo:1168; depth 51.19 2024-11-06T04:43:09Z- handleDVL() - DVL msg epochMillisec: 1730868189.10 2024-11-06T04:43:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:09Z- handleDvl() - RDI_Pathfinder msg: 1730868189.10 sec; seqNo:385; #beams:0 2024-11-06T04:43:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.13 2024-11-06T04:43:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868189.12 epoch sec; seqNo:1168; 4061667.80 north; 594317.31 east; 36.70 lat; -121.94 lon; 51.19 depth; utmZone: 10 2024-11-06T04:43:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868188.30 2024-11-06T04:43:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:09Z- waiting for 1730868192.30, time is 1730868189.12 2024-11-06T04:43:09Z- AHRS_M2 msg: 1730868189.49 epoch sec; seqNo:1072 2024-11-06T04:43:09Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.49 psi 2024-11-06T04:43:09Z- handleDepth()- Depth_Keller msg: 1730868189.50 epoch sec; seqNo:1169; depth 51.18 2024-11-06T04:43:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868189.51 epoch sec; seqNo:1169; 4061667.80 north; 594317.31 east; 36.70 lat; -121.94 lon; 51.18 depth; utmZone: 10 2024-11-06T04:43:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868188.30 2024-11-06T04:43:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:09Z- waiting for 1730868192.30, time is 1730868189.51 2024-11-06T04:43:09Z- AHRS_M2 msg: 1730868189.90 epoch sec; seqNo:1073 2024-11-06T04:43:09Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.49 psi 2024-11-06T04:43:09Z- handleDepth()- Depth_Keller msg: 1730868189.90 epoch sec; seqNo:1170; depth 51.20 2024-11-06T04:43:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868189.91 epoch sec; seqNo:1170; 4061667.80 north; 594317.31 east; 36.70 lat; -121.94 lon; 51.20 depth; utmZone: 10 2024-11-06T04:43:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868188.30 2024-11-06T04:43:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:10Z- waiting for 1730868192.30, time is 1730868189.91 2024-11-06T04:43:10Z- AHRS_M2 msg: 1730868190.30 epoch sec; seqNo:1074 2024-11-06T04:43:10Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.49 psi 2024-11-06T04:43:10Z- handleDepth()- Depth_Keller msg: 1730868190.31 epoch sec; seqNo:1171; depth 51.19 2024-11-06T04:43:10Z- handleDVL() - DVL msg epochMillisec: 1730868190.31 2024-11-06T04:43:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:10Z- handleDvl() - RDI_Pathfinder msg: 1730868190.31 sec; seqNo:386; #beams:0 2024-11-06T04:43:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.13 2024-11-06T04:43:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868190.33 epoch sec; seqNo:1171; 4061667.80 north; 594317.31 east; 36.70 lat; -121.94 lon; 51.19 depth; utmZone: 10 2024-11-06T04:43:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868188.30 2024-11-06T04:43:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:10Z- waiting for 1730868192.30, time is 1730868190.33 2024-11-06T04:43:10Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:43:10Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:43:10Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:43:10Z- AHRS_M2 msg: 1730868190.69 epoch sec; seqNo:1075 2024-11-06T04:43:10Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.49 psi 2024-11-06T04:43:10Z- handleDepth()- Depth_Keller msg: 1730868190.73 epoch sec; seqNo:1172; depth 51.18 2024-11-06T04:43:10Z- handleDVL() - DVL msg epochMillisec: 1730868190.75 2024-11-06T04:43:10Z- handleDvl() - RDI_Pathfinder msg: 1730868190.75 sec; seqNo:387; #beams:4 2024-11-06T04:43:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.55 , 38.50 , 39.06 , 41.12 2024-11-06T04:43:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.01 , 0.06 2024-11-06T04:43:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868190.76 epoch sec; seqNo:1172; 4061667.81 north; 594318.53 east; 36.70 lat; -121.94 lon; 51.18 depth; utmZone: 10 2024-11-06T04:43:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868188.30 2024-11-06T04:43:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:10Z- waiting for 1730868192.30, time is 1730868190.76 2024-11-06T04:43:11Z- AHRS_M2 msg: 1730868191.11 epoch sec; seqNo:1076 2024-11-06T04:43:11Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.49 psi 2024-11-06T04:43:11Z- handleDepth()- Depth_Keller msg: 1730868191.11 epoch sec; seqNo:1173; depth 51.18 2024-11-06T04:43:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868191.13 epoch sec; seqNo:1173; 4061667.81 north; 594318.53 east; 36.70 lat; -121.94 lon; 51.18 depth; utmZone: 10 2024-11-06T04:43:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868188.30 2024-11-06T04:43:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:11Z- waiting for 1730868192.30, time is 1730868191.13 2024-11-06T04:43:11Z- AHRS_M2 msg: 1730868191.51 epoch sec; seqNo:1077 2024-11-06T04:43:11Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.49 psi 2024-11-06T04:43:11Z- handleDepth()- Depth_Keller msg: 1730868191.53 epoch sec; seqNo:1174; depth 51.18 2024-11-06T04:43:11Z- handleDVL() - DVL msg epochMillisec: 1730868191.53 2024-11-06T04:43:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:11Z- handleDvl() - RDI_Pathfinder msg: 1730868191.53 sec; seqNo:388; #beams:0 2024-11-06T04:43:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.01 , 0.06 2024-11-06T04:43:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868191.54 epoch sec; seqNo:1174; 4061668.19 north; 594318.53 east; 36.70 lat; -121.94 lon; 51.18 depth; utmZone: 10 2024-11-06T04:43:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868188.30 2024-11-06T04:43:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:11Z- waiting for 1730868192.30, time is 1730868191.54 2024-11-06T04:43:11Z- AHRS_M2 msg: 1730868191.92 epoch sec; seqNo:1078 2024-11-06T04:43:11Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.50 psi 2024-11-06T04:43:11Z- handleDepth()- Depth_Keller msg: 1730868191.92 epoch sec; seqNo:1175; depth 51.16 2024-11-06T04:43:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868191.94 epoch sec; seqNo:1175; 4061668.21 north; 594319.75 east; 36.70 lat; -121.94 lon; 51.16 depth; utmZone: 10 2024-11-06T04:43:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868188.30 2024-11-06T04:43:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:12Z- waiting for 1730868192.30, time is 1730868191.94 2024-11-06T04:43:12Z- AHRS_M2 msg: 1730868192.33 epoch sec; seqNo:1079 2024-11-06T04:43:12Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.50 psi 2024-11-06T04:43:12Z- handleDepth()- Depth_Keller msg: 1730868192.33 epoch sec; seqNo:1176; depth 51.16 2024-11-06T04:43:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868192.39 epoch sec; seqNo:1176; 4061668.21 north; 594319.75 east; 36.70 lat; -121.94 lon; 51.16 depth; utmZone: 10 2024-11-06T04:43:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868188.30 2024-11-06T04:43:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:12Z- waiting for 1730868192.30, time is 1730868192.39 2024-11-06T04:43:12Z- LcmTrn::updateTrn() >>>> pose time 1730868192.39, meas time 1730868191.53 <<<< 2024-11-06T04:43:12Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868192.39 2024-11-06T04:43:12Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:43:12Z- TerrainNav::motionUpdate() - currEstimate = (4061668, 594319.75, 51.16) 2024-11-06T04:43:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:43:12Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:43:12Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:43:12Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:43:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:43:12Z- variances([0], [2], and psi_berg) = 0.180226 0.183586 0.000000 2024-11-06T04:43:12Z- publishEstimatesDecoder() - at 1730868192.39 sec 2024-11-06T04:43:12Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:43:12Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061610.98 594322.90 56.14 2024-11-06T04:43:12Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061612.17 594323.84 56.22 lat/lon: 36.70 -121.94 2024-11-06T04:43:12Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.18 1.48 0.00 2024-11-06T04:43:12Z- AHRS_M2 msg: 1730868192.72 epoch sec; seqNo:1080 2024-11-06T04:43:12Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.50 psi 2024-11-06T04:43:12Z- handleDepth()- Depth_Keller msg: 1730868192.74 epoch sec; seqNo:1177; depth 51.13 2024-11-06T04:43:12Z- handleDVL() - DVL msg epochMillisec: 1730868192.76 2024-11-06T04:43:12Z- handleDvl() - RDI_Pathfinder msg: 1730868192.76 sec; seqNo:389; #beams:4 2024-11-06T04:43:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.55 , 38.31 , 39.81 , 41.12 2024-11-06T04:43:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.02 , -0.05 2024-11-06T04:43:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868192.78 epoch sec; seqNo:1177; 4061668.21 north; 594319.75 east; 36.70 lat; -121.94 lon; 51.13 depth; utmZone: 10 2024-11-06T04:43:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868192.39 2024-11-06T04:43:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:12Z- waiting for 1730868196.39, time is 1730868192.78 2024-11-06T04:43:13Z- AHRS_M2 msg: 1730868193.13 epoch sec; seqNo:1081 2024-11-06T04:43:13Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.50 psi 2024-11-06T04:43:13Z- handleDepth()- Depth_Keller msg: 1730868193.13 epoch sec; seqNo:1178; depth 51.11 2024-11-06T04:43:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868193.16 epoch sec; seqNo:1178; 4061668.22 north; 594320.97 east; 36.70 lat; -121.94 lon; 51.11 depth; utmZone: 10 2024-11-06T04:43:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868192.39 2024-11-06T04:43:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:13Z- waiting for 1730868196.39, time is 1730868193.16 2024-11-06T04:43:13Z- AHRS_M2 msg: 1730868193.54 epoch sec; seqNo:1082 2024-11-06T04:43:13Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.50 psi 2024-11-06T04:43:13Z- handleDepth()- Depth_Keller msg: 1730868193.54 epoch sec; seqNo:1179; depth 51.10 2024-11-06T04:43:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868193.55 epoch sec; seqNo:1179; 4061668.22 north; 594320.97 east; 36.70 lat; -121.94 lon; 51.10 depth; utmZone: 10 2024-11-06T04:43:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868192.39 2024-11-06T04:43:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:13Z- waiting for 1730868196.39, time is 1730868193.55 2024-11-06T04:43:13Z- AHRS_M2 msg: 1730868193.94 epoch sec; seqNo:1083 2024-11-06T04:43:13Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.50 psi 2024-11-06T04:43:13Z- handleDepth()- Depth_Keller msg: 1730868193.95 epoch sec; seqNo:1180; depth 51.08 2024-11-06T04:43:13Z- handleDVL() - DVL msg epochMillisec: 1730868193.95 2024-11-06T04:43:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:13Z- handleDvl() - RDI_Pathfinder msg: 1730868193.95 sec; seqNo:390; #beams:0 2024-11-06T04:43:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.98 , -0.02 , -0.05 2024-11-06T04:43:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868193.96 epoch sec; seqNo:1180; 4061668.22 north; 594320.97 east; 36.70 lat; -121.94 lon; 51.08 depth; utmZone: 10 2024-11-06T04:43:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868192.39 2024-11-06T04:43:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:14Z- waiting for 1730868196.39, time is 1730868193.96 2024-11-06T04:43:14Z- AHRS_M2 msg: 1730868194.37 epoch sec; seqNo:1084 2024-11-06T04:43:14Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.50 psi 2024-11-06T04:43:14Z- handleDepth()- Depth_Keller msg: 1730868194.38 epoch sec; seqNo:1181; depth 51.05 2024-11-06T04:43:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868194.42 epoch sec; seqNo:1181; 4061668.23 north; 594322.19 east; 36.70 lat; -121.94 lon; 51.05 depth; utmZone: 10 2024-11-06T04:43:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868192.39 2024-11-06T04:43:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:14Z- waiting for 1730868196.39, time is 1730868194.42 2024-11-06T04:43:14Z- AHRS_M2 msg: 1730868194.74 epoch sec; seqNo:1085 2024-11-06T04:43:14Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.50 psi 2024-11-06T04:43:14Z- handleDepth()- Depth_Keller msg: 1730868194.75 epoch sec; seqNo:1182; depth 51.03 2024-11-06T04:43:14Z- handleDVL() - DVL msg epochMillisec: 1730868194.78 2024-11-06T04:43:14Z- handleDvl() - RDI_Pathfinder msg: 1730868194.78 sec; seqNo:391; #beams:4 2024-11-06T04:43:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.55 , 38.13 , 39.25 , 40.56 2024-11-06T04:43:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , -0.09 2024-11-06T04:43:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868194.79 epoch sec; seqNo:1182; 4061668.23 north; 594322.19 east; 36.70 lat; -121.94 lon; 51.03 depth; utmZone: 10 2024-11-06T04:43:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868192.39 2024-11-06T04:43:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:14Z- waiting for 1730868196.39, time is 1730868194.79 2024-11-06T04:43:15Z- AHRS_M2 msg: 1730868195.15 epoch sec; seqNo:1086 2024-11-06T04:43:15Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.50 psi 2024-11-06T04:43:15Z- handleDepth()- Depth_Keller msg: 1730868195.15 epoch sec; seqNo:1183; depth 51.01 2024-11-06T04:43:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868195.17 epoch sec; seqNo:1183; 4061668.23 north; 594322.19 east; 36.70 lat; -121.94 lon; 51.01 depth; utmZone: 10 2024-11-06T04:43:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868192.39 2024-11-06T04:43:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:15Z- waiting for 1730868196.39, time is 1730868195.17 2024-11-06T04:43:15Z- AHRS_M2 msg: 1730868195.55 epoch sec; seqNo:1087 2024-11-06T04:43:15Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.50 psi 2024-11-06T04:43:15Z- handleDepth()- Depth_Keller msg: 1730868195.56 epoch sec; seqNo:1184; depth 50.99 2024-11-06T04:43:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868195.57 epoch sec; seqNo:1184; 4061668.23 north; 594322.19 east; 36.70 lat; -121.94 lon; 50.99 depth; utmZone: 10 2024-11-06T04:43:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868192.39 2024-11-06T04:43:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:15Z- waiting for 1730868196.39, time is 1730868195.57 2024-11-06T04:43:15Z- AHRS_M2 msg: 1730868195.95 epoch sec; seqNo:1088 2024-11-06T04:43:15Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.50 psi 2024-11-06T04:43:15Z- handleDepth()- Depth_Keller msg: 1730868195.97 epoch sec; seqNo:1185; depth 50.96 2024-11-06T04:43:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868195.98 epoch sec; seqNo:1185; 4061668.63 north; 594323.41 east; 36.70 lat; -121.94 lon; 50.96 depth; utmZone: 10 2024-11-06T04:43:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868192.39 2024-11-06T04:43:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:16Z- waiting for 1730868196.39, time is 1730868195.98 2024-11-06T04:43:16Z- AHRS_M2 msg: 1730868196.39 epoch sec; seqNo:1089 2024-11-06T04:43:16Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.50 psi 2024-11-06T04:43:16Z- handleDepth()- Depth_Keller msg: 1730868196.41 epoch sec; seqNo:1186; depth 50.93 2024-11-06T04:43:16Z- handleDVL() - DVL msg epochMillisec: 1730868196.42 2024-11-06T04:43:16Z- handleDvl() - RDI_Pathfinder msg: 1730868196.42 sec; seqNo:392; #beams:4 2024-11-06T04:43:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.18 , 38.13 , 38.50 , 40.75 2024-11-06T04:43:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.06 , -0.06 2024-11-06T04:43:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868196.46 epoch sec; seqNo:1186; 4061668.63 north; 594323.41 east; 36.70 lat; -121.94 lon; 50.93 depth; utmZone: 10 2024-11-06T04:43:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868192.39 2024-11-06T04:43:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:16Z- waiting for 1730868196.39, time is 1730868196.46 2024-11-06T04:43:16Z- LcmTrn::updateTrn() >>>> pose time 1730868196.46, meas time 1730868196.42 <<<< 2024-11-06T04:43:16Z- LcmTrn::updateTrn() equating pose 1730868196.42 and meas times 1730868196.42 2024-11-06T04:43:16Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868196.42 2024-11-06T04:43:16Z- TerrainNav::motionUpdate() - currEstimate = (4061669, 594323.41, 50.93) 2024-11-06T04:43:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:43:16Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:43:16Z- TNavPF::Projecting Measurements 2024-11-06T04:43:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:43:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:43:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:43:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:43:16Z- TNavFilter::4 good beams. 2024-11-06T04:43:16Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:43:16Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:43:16Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:43:16Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:43:16Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:43:16Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:43:16Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:43:16Z- TerrainMapOctree: (vehN, vehE ) = (4061668.63, 594323.41). 2024-11-06T04:43:16Z- TerrainMapOctree: Min Distance = 109.59. 2024-11-06T04:43:16Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:43:16Z- Forcing Subcloud Comparison 2024-11-06T04:43:16Z- Weighting particles with subcloud comparison 2024-11-06T04:43:16Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:16Z- meanExpectedMeasDiff: 0.313934 delta_rms_squared: 2.688309 alpha: 0.992615 2024-11-06T04:43:16Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:16Z- meanExpectedMeasDiff: -0.326043 delta_rms_squared: 1.998935 alpha: 0.990094 2024-11-06T04:43:16Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:16Z- meanExpectedMeasDiff: -0.128100 delta_rms_squared: 2.175995 alpha: 0.990893 2024-11-06T04:43:16Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:16Z- meanExpectedMeasDiff: -0.338187 delta_rms_squared: 2.284486 alpha: 0.991321 2024-11-06T04:43:16Z- Subcloud NIS: 0.156165 2024-11-06T04:43:16Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:43:16Z- TNavPF:: sumWeights = 0.546838 2024-11-06T04:43:16Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:43:16Z- TNavPF::First Term of Map Covariance Matrix is 0.49 2024-11-06T04:43:16Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T04:43:16Z- TerrainNav::Resampling particle filter... 2024-11-06T04:43:16Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868196.42 sec, ping # 0. 2024-11-06T04:43:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:43:16Z- variances([0], [2], and psi_berg) = 0.181371 0.185321 0.000000 2024-11-06T04:43:16Z- publishEstimatesDecoder() - at 1730868196.42 sec 2024-11-06T04:43:16Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:43:16Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061612.72 594327.86 55.64 2024-11-06T04:43:16Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061612.59 594327.50 56.05 lat/lon: 36.70 -121.94 2024-11-06T04:43:16Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.19 0.55 0.00 2024-11-06T04:43:16Z- AHRS_M2 msg: 1730868196.77 epoch sec; seqNo:1090 2024-11-06T04:43:16Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.50 psi 2024-11-06T04:43:16Z- handleDepth()- Depth_Keller msg: 1730868196.79 epoch sec; seqNo:1187; depth 50.90 2024-11-06T04:43:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868196.84 epoch sec; seqNo:1187; 4061668.63 north; 594323.41 east; 36.70 lat; -121.94 lon; 50.90 depth; utmZone: 10 2024-11-06T04:43:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868196.42 2024-11-06T04:43:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:16Z- waiting for 1730868200.42, time is 1730868196.84 2024-11-06T04:43:17Z- AHRS_M2 msg: 1730868197.17 epoch sec; seqNo:1091 2024-11-06T04:43:17Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.49 psi 2024-11-06T04:43:17Z- handleDepth()- Depth_Keller msg: 1730868197.17 epoch sec; seqNo:1188; depth 50.88 2024-11-06T04:43:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868197.19 epoch sec; seqNo:1188; 4061668.64 north; 594324.63 east; 36.70 lat; -121.94 lon; 50.88 depth; utmZone: 10 2024-11-06T04:43:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868196.42 2024-11-06T04:43:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:17Z- waiting for 1730868200.42, time is 1730868197.19 2024-11-06T04:43:17Z- AHRS_M2 msg: 1730868197.57 epoch sec; seqNo:1092 2024-11-06T04:43:17Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.49 psi 2024-11-06T04:43:17Z- handleDepth()- Depth_Keller msg: 1730868197.58 epoch sec; seqNo:1189; depth 50.87 2024-11-06T04:43:17Z- handleDVL() - DVL msg epochMillisec: 1730868197.58 2024-11-06T04:43:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:17Z- handleDvl() - RDI_Pathfinder msg: 1730868197.58 sec; seqNo:393; #beams:0 2024-11-06T04:43:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.06 , -0.06 2024-11-06T04:43:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868197.59 epoch sec; seqNo:1189; 4061668.64 north; 594324.63 east; 36.70 lat; -121.94 lon; 50.87 depth; utmZone: 10 2024-11-06T04:43:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868196.42 2024-11-06T04:43:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:17Z- waiting for 1730868200.42, time is 1730868197.59 2024-11-06T04:43:17Z- AHRS_M2 msg: 1730868197.98 epoch sec; seqNo:1093 2024-11-06T04:43:17Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.49 psi 2024-11-06T04:43:17Z- handleDepth()- Depth_Keller msg: 1730868197.98 epoch sec; seqNo:1190; depth 50.84 2024-11-06T04:43:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868197.99 epoch sec; seqNo:1190; 4061668.64 north; 594324.63 east; 36.70 lat; -121.94 lon; 50.84 depth; utmZone: 10 2024-11-06T04:43:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868196.42 2024-11-06T04:43:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:18Z- waiting for 1730868200.42, time is 1730868197.99 2024-11-06T04:43:18Z- AHRS_M2 msg: 1730868198.41 epoch sec; seqNo:1094 2024-11-06T04:43:18Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.49 psi 2024-11-06T04:43:18Z- handleDepth()- Depth_Keller msg: 1730868198.41 epoch sec; seqNo:1191; depth 50.81 2024-11-06T04:43:18Z- handleDVL() - DVL msg epochMillisec: 1730868198.43 2024-11-06T04:43:18Z- handleDvl() - RDI_Pathfinder msg: 1730868198.43 sec; seqNo:394; #beams:4 2024-11-06T04:43:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.55 , 37.57 , 39.06 , 40.75 2024-11-06T04:43:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.06 , -0.00 2024-11-06T04:43:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868198.46 epoch sec; seqNo:1191; 4061668.64 north; 594324.63 east; 36.70 lat; -121.94 lon; 50.81 depth; utmZone: 10 2024-11-06T04:43:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868196.42 2024-11-06T04:43:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:18Z- waiting for 1730868200.42, time is 1730868198.46 2024-11-06T04:43:18Z- AHRS_M2 msg: 1730868198.78 epoch sec; seqNo:1095 2024-11-06T04:43:18Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.49 psi 2024-11-06T04:43:18Z- handleDepth()- Depth_Keller msg: 1730868198.79 epoch sec; seqNo:1192; depth 50.80 2024-11-06T04:43:18Z- handleDVL() - DVL msg epochMillisec: 1730868198.79 2024-11-06T04:43:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:18Z- handleDvl() - RDI_Pathfinder msg: 1730868198.79 sec; seqNo:395; #beams:0 2024-11-06T04:43:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.06 , -0.00 2024-11-06T04:43:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868198.82 epoch sec; seqNo:1192; 4061668.65 north; 594325.85 east; 36.70 lat; -121.94 lon; 50.80 depth; utmZone: 10 2024-11-06T04:43:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868196.42 2024-11-06T04:43:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:18Z- waiting for 1730868200.42, time is 1730868198.82 2024-11-06T04:43:19Z- AHRS_M2 msg: 1730868199.19 epoch sec; seqNo:1096 2024-11-06T04:43:19Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.49 psi 2024-11-06T04:43:19Z- handleDepth()- Depth_Keller msg: 1730868199.19 epoch sec; seqNo:1193; depth 50.79 2024-11-06T04:43:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868199.20 epoch sec; seqNo:1193; 4061669.03 north; 594325.84 east; 36.70 lat; -121.94 lon; 50.79 depth; utmZone: 10 2024-11-06T04:43:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868196.42 2024-11-06T04:43:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:19Z- waiting for 1730868200.42, time is 1730868199.20 2024-11-06T04:43:19Z- handleDepth()- Depth_Keller msg: 1730868199.61 epoch sec; seqNo:1194; depth 50.78 2024-11-06T04:43:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868199.62 epoch sec; seqNo:1194; 4061669.03 north; 594325.84 east; 36.70 lat; -121.94 lon; 50.78 depth; utmZone: 10 2024-11-06T04:43:19Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:43:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868196.42 2024-11-06T04:43:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:19Z- waiting for 1730868200.42, time is 1730868199.62 2024-11-06T04:43:20Z- AHRS_M2 msg: 1730868199.99 epoch sec; seqNo:1097 2024-11-06T04:43:20Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:20Z- handleDepth()- Depth_Keller msg: 1730868200.00 epoch sec; seqNo:1195; depth 50.78 2024-11-06T04:43:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868200.02 epoch sec; seqNo:1195; 4061669.03 north; 594325.84 east; 36.70 lat; -121.94 lon; 50.78 depth; utmZone: 10 2024-11-06T04:43:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868196.42 2024-11-06T04:43:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:20Z- waiting for 1730868200.42, time is 1730868200.02 2024-11-06T04:43:20Z- AHRS_M2 msg: 1730868200.41 epoch sec; seqNo:1098 2024-11-06T04:43:20Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:20Z- handleDepth()- Depth_Keller msg: 1730868200.43 epoch sec; seqNo:1196; depth 50.76 2024-11-06T04:43:20Z- handleDVL() - DVL msg epochMillisec: 1730868200.48 2024-11-06T04:43:20Z- handleDvl() - RDI_Pathfinder msg: 1730868200.48 sec; seqNo:396; #beams:4 2024-11-06T04:43:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.99 , 37.57 , 39.81 , 40.75 2024-11-06T04:43:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.05 , 0.03 2024-11-06T04:43:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868200.49 epoch sec; seqNo:1196; 4061669.05 north; 594327.06 east; 36.70 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T04:43:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868196.42 2024-11-06T04:43:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:20Z- waiting for 1730868200.42, time is 1730868200.49 2024-11-06T04:43:20Z- LcmTrn::updateTrn() >>>> pose time 1730868200.49, meas time 1730868200.48 <<<< 2024-11-06T04:43:20Z- LcmTrn::updateTrn() equating pose 1730868200.48 and meas times 1730868200.48 2024-11-06T04:43:20Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868200.48 2024-11-06T04:43:20Z- TerrainNav::motionUpdate() - currEstimate = (4061669, 594327.06, 50.76) 2024-11-06T04:43:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:43:20Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:43:20Z- TNavPF::Projecting Measurements 2024-11-06T04:43:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:43:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:43:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:43:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:43:20Z- TNavFilter::4 good beams. 2024-11-06T04:43:20Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:43:20Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:43:20Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:43:20Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:43:20Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:43:20Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:43:20Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:43:20Z- TerrainMapOctree: (vehN, vehE ) = (4061669.05, 594327.06). 2024-11-06T04:43:20Z- TerrainMapOctree: Min Distance = 111.77. 2024-11-06T04:43:20Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:43:20Z- Forcing Subcloud Comparison 2024-11-06T04:43:20Z- Weighting particles with subcloud comparison 2024-11-06T04:43:20Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:20Z- meanExpectedMeasDiff: 1.308906 delta_rms_squared: 1.213165 alpha: 0.983782 2024-11-06T04:43:20Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:20Z- meanExpectedMeasDiff: -1.044355 delta_rms_squared: 0.916827 alpha: 0.978651 2024-11-06T04:43:20Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:20Z- meanExpectedMeasDiff: 2.073248 delta_rms_squared: 1.080580 alpha: 0.981828 2024-11-06T04:43:20Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:20Z- meanExpectedMeasDiff: -0.328459 delta_rms_squared: 1.038323 alpha: 0.981102 2024-11-06T04:43:20Z- Subcloud NIS: 0.154266 2024-11-06T04:43:20Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:43:20Z- TNavPF:: sumWeights = 0.331945 2024-11-06T04:43:20Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:43:20Z- TNavPF::First Term of Map Covariance Matrix is 0.37 2024-11-06T04:43:20Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T04:43:20Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868200.48 sec, ping # 0. 2024-11-06T04:43:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:43:20Z- variances([0], [2], and psi_berg) = 0.186498 0.190005 0.000000 2024-11-06T04:43:20Z- publishEstimatesDecoder() - at 1730868200.48 sec 2024-11-06T04:43:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:43:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061611.76 594330.15 56.24 2024-11-06T04:43:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061613.00 594331.15 55.66 lat/lon: 36.70 -121.94 2024-11-06T04:43:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.19 0.19 0.40 0.00 2024-11-06T04:43:20Z- AHRS_M2 msg: 1730868200.80 epoch sec; seqNo:1099 2024-11-06T04:43:20Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:20Z- handleDepth()- Depth_Keller msg: 1730868200.82 epoch sec; seqNo:1197; depth 50.75 2024-11-06T04:43:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868200.85 epoch sec; seqNo:1197; 4061669.05 north; 594327.06 east; 36.70 lat; -121.94 lon; 50.75 depth; utmZone: 10 2024-11-06T04:43:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868200.48 2024-11-06T04:43:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:20Z- waiting for 1730868204.48, time is 1730868200.85 2024-11-06T04:43:21Z- AHRS_M2 msg: 1730868201.21 epoch sec; seqNo:1100 2024-11-06T04:43:21Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:21Z- handleDepth()- Depth_Keller msg: 1730868201.21 epoch sec; seqNo:1198; depth 50.75 2024-11-06T04:43:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868201.22 epoch sec; seqNo:1198; 4061669.05 north; 594327.06 east; 36.70 lat; -121.94 lon; 50.75 depth; utmZone: 10 2024-11-06T04:43:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868200.48 2024-11-06T04:43:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:21Z- waiting for 1730868204.48, time is 1730868201.22 2024-11-06T04:43:21Z- AHRS_M2 msg: 1730868201.61 epoch sec; seqNo:1101 2024-11-06T04:43:21Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:21Z- handleDepth()- Depth_Keller msg: 1730868201.62 epoch sec; seqNo:1199; depth 50.75 2024-11-06T04:43:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868201.62 epoch sec; seqNo:1199; 4061669.05 north; 594327.06 east; 36.70 lat; -121.94 lon; 50.75 depth; utmZone: 10 2024-11-06T04:43:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868200.48 2024-11-06T04:43:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:21Z- waiting for 1730868204.48, time is 1730868201.62 2024-11-06T04:43:22Z- AHRS_M2 msg: 1730868202.02 epoch sec; seqNo:1102 2024-11-06T04:43:22Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:22Z- handleDepth()- Depth_Keller msg: 1730868202.02 epoch sec; seqNo:1200; depth 50.74 2024-11-06T04:43:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868202.04 epoch sec; seqNo:1200; 4061669.06 north; 594328.28 east; 36.70 lat; -121.94 lon; 50.74 depth; utmZone: 10 2024-11-06T04:43:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868200.48 2024-11-06T04:43:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:22Z- waiting for 1730868204.48, time is 1730868202.04 2024-11-06T04:43:22Z- AHRS_M2 msg: 1730868202.42 epoch sec; seqNo:1103 2024-11-06T04:43:22Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:22Z- handleDepth()- Depth_Keller msg: 1730868202.43 epoch sec; seqNo:1201; depth 50.74 2024-11-06T04:43:22Z- handleDVL() - DVL msg epochMillisec: 1730868202.46 2024-11-06T04:43:22Z- handleDvl() - RDI_Pathfinder msg: 1730868202.46 sec; seqNo:397; #beams:4 2024-11-06T04:43:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.17 , 37.38 , 39.62 , 40.93 2024-11-06T04:43:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , 0.03 2024-11-06T04:43:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868202.47 epoch sec; seqNo:1201; 4061669.44 north; 594328.28 east; 36.70 lat; -121.94 lon; 50.74 depth; utmZone: 10 2024-11-06T04:43:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868200.48 2024-11-06T04:43:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:22Z- waiting for 1730868204.48, time is 1730868202.47 2024-11-06T04:43:22Z- AHRS_M2 msg: 1730868202.83 epoch sec; seqNo:1104 2024-11-06T04:43:22Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:22Z- handleDepth()- Depth_Keller msg: 1730868202.84 epoch sec; seqNo:1202; depth 50.74 2024-11-06T04:43:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868202.85 epoch sec; seqNo:1202; 4061669.44 north; 594328.28 east; 36.70 lat; -121.94 lon; 50.74 depth; utmZone: 10 2024-11-06T04:43:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868200.48 2024-11-06T04:43:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:22Z- waiting for 1730868204.48, time is 1730868202.85 2024-11-06T04:43:23Z- AHRS_M2 msg: 1730868203.23 epoch sec; seqNo:1105 2024-11-06T04:43:23Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:23Z- handleDepth()- Depth_Keller msg: 1730868203.23 epoch sec; seqNo:1203; depth 50.73 2024-11-06T04:43:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868203.25 epoch sec; seqNo:1203; 4061669.44 north; 594328.28 east; 36.70 lat; -121.94 lon; 50.73 depth; utmZone: 10 2024-11-06T04:43:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868200.48 2024-11-06T04:43:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:23Z- waiting for 1730868204.48, time is 1730868203.25 2024-11-06T04:43:23Z- AHRS_M2 msg: 1730868203.63 epoch sec; seqNo:1106 2024-11-06T04:43:23Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:23Z- handleDepth()- Depth_Keller msg: 1730868203.64 epoch sec; seqNo:1204; depth 50.74 2024-11-06T04:43:23Z- handleDVL() - DVL msg epochMillisec: 1730868203.64 2024-11-06T04:43:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:23Z- handleDvl() - RDI_Pathfinder msg: 1730868203.64 sec; seqNo:398; #beams:0 2024-11-06T04:43:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , 0.03 2024-11-06T04:43:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868203.67 epoch sec; seqNo:1204; 4061669.45 north; 594329.50 east; 36.70 lat; -121.94 lon; 50.74 depth; utmZone: 10 2024-11-06T04:43:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868200.48 2024-11-06T04:43:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:23Z- waiting for 1730868204.48, time is 1730868203.67 2024-11-06T04:43:24Z- AHRS_M2 msg: 1730868204.04 epoch sec; seqNo:1107 2024-11-06T04:43:24Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:24Z- handleDepth()- Depth_Keller msg: 1730868204.04 epoch sec; seqNo:1205; depth 50.73 2024-11-06T04:43:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868204.05 epoch sec; seqNo:1205; 4061669.45 north; 594329.50 east; 36.70 lat; -121.94 lon; 50.73 depth; utmZone: 10 2024-11-06T04:43:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868200.48 2024-11-06T04:43:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:24Z- waiting for 1730868204.48, time is 1730868204.05 2024-11-06T04:43:24Z- AHRS_M2 msg: 1730868204.44 epoch sec; seqNo:1108 2024-11-06T04:43:24Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:24Z- handleDepth()- Depth_Keller msg: 1730868204.45 epoch sec; seqNo:1206; depth 50.72 2024-11-06T04:43:24Z- handleDVL() - DVL msg epochMillisec: 1730868204.48 2024-11-06T04:43:24Z- handleDvl() - RDI_Pathfinder msg: 1730868204.48 sec; seqNo:399; #beams:4 2024-11-06T04:43:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.80 , 37.94 , 39.06 , 40.37 2024-11-06T04:43:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.03 2024-11-06T04:43:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868204.50 epoch sec; seqNo:1206; 4061669.45 north; 594329.50 east; 36.70 lat; -121.94 lon; 50.72 depth; utmZone: 10 2024-11-06T04:43:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868200.48 2024-11-06T04:43:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:24Z- waiting for 1730868204.48, time is 1730868204.50 2024-11-06T04:43:24Z- LcmTrn::updateTrn() >>>> pose time 1730868204.50, meas time 1730868204.48 <<<< 2024-11-06T04:43:24Z- LcmTrn::updateTrn() equating pose 1730868204.48 and meas times 1730868204.48 2024-11-06T04:43:24Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868204.48 2024-11-06T04:43:24Z- TerrainNav::motionUpdate() - currEstimate = (4061669, 594329.50, 50.72) 2024-11-06T04:43:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:43:24Z- TerrainNav:: SubcloudNIS is 0.15, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:43:24Z- TNavPF::Projecting Measurements 2024-11-06T04:43:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:43:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:43:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:43:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:43:24Z- TNavFilter::4 good beams. 2024-11-06T04:43:24Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:43:24Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:43:24Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:43:24Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:43:24Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:43:24Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:43:24Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:43:24Z- TerrainMapOctree: (vehN, vehE ) = (4061669.45, 594329.50). 2024-11-06T04:43:24Z- TerrainMapOctree: Min Distance = 113.17. 2024-11-06T04:43:24Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:43:24Z- Forcing Subcloud Comparison 2024-11-06T04:43:24Z- Weighting particles with subcloud comparison 2024-11-06T04:43:24Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:24Z- meanExpectedMeasDiff: 1.694690 delta_rms_squared: 0.677929 alpha: 0.971344 2024-11-06T04:43:24Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:24Z- meanExpectedMeasDiff: -0.330235 delta_rms_squared: 0.918026 alpha: 0.978679 2024-11-06T04:43:24Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:24Z- meanExpectedMeasDiff: 1.063564 delta_rms_squared: 0.735433 alpha: 0.973525 2024-11-06T04:43:24Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:24Z- meanExpectedMeasDiff: -0.919228 delta_rms_squared: 0.845427 alpha: 0.976890 2024-11-06T04:43:24Z- Subcloud NIS: 0.153757 2024-11-06T04:43:24Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:43:24Z- TNavPF:: sumWeights = 0.446087 2024-11-06T04:43:24Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:43:24Z- TNavPF::First Term of Map Covariance Matrix is 0.54 2024-11-06T04:43:24Z- TNavPF::Calculated NIS Value : 0.27 numBeams normalized NIS: 0.07 2024-11-06T04:43:24Z- TerrainNav::Resampling particle filter... 2024-11-06T04:43:24Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868204.48 sec, ping # 0. 2024-11-06T04:43:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:43:24Z- variances([0], [2], and psi_berg) = 0.183764 0.187414 0.000000 2024-11-06T04:43:24Z- publishEstimatesDecoder() - at 1730868204.48 sec 2024-11-06T04:43:24Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:43:24Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061613.51 594334.05 55.07 2024-11-06T04:43:24Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061613.37 594333.58 55.53 lat/lon: 36.70 -121.94 2024-11-06T04:43:24Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.19 0.40 0.00 2024-11-06T04:43:24Z- AHRS_M2 msg: 1730868204.85 epoch sec; seqNo:1109 2024-11-06T04:43:24Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:24Z- handleDepth()- Depth_Keller msg: 1730868204.85 epoch sec; seqNo:1207; depth 50.70 2024-11-06T04:43:24Z- handleDVL() - DVL msg epochMillisec: 1730868204.86 2024-11-06T04:43:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:24Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:24Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:24Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:24Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:24Z- handleDvl() - RDI_Pathfinder msg: 1730868204.86 sec; seqNo:400; #beams:0 2024-11-06T04:43:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.03 2024-11-06T04:43:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868204.89 epoch sec; seqNo:1207; 4061669.46 north; 594330.72 east; 36.70 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T04:43:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868204.48 2024-11-06T04:43:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:25Z- waiting for 1730868208.48, time is 1730868204.89 2024-11-06T04:43:25Z- AHRS_M2 msg: 1730868205.25 epoch sec; seqNo:1110 2024-11-06T04:43:25Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:25Z- handleDepth()- Depth_Keller msg: 1730868205.25 epoch sec; seqNo:1208; depth 50.70 2024-11-06T04:43:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868205.26 epoch sec; seqNo:1208; 4061669.84 north; 594330.72 east; 36.70 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T04:43:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868204.48 2024-11-06T04:43:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:25Z- waiting for 1730868208.48, time is 1730868205.26 2024-11-06T04:43:25Z- AHRS_M2 msg: 1730868205.65 epoch sec; seqNo:1111 2024-11-06T04:43:25Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:25Z- handleDepth()- Depth_Keller msg: 1730868205.66 epoch sec; seqNo:1209; depth 50.69 2024-11-06T04:43:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868205.67 epoch sec; seqNo:1209; 4061669.84 north; 594330.72 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T04:43:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868204.48 2024-11-06T04:43:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:25Z- waiting for 1730868208.48, time is 1730868205.67 2024-11-06T04:43:26Z- AHRS_M2 msg: 1730868206.06 epoch sec; seqNo:1112 2024-11-06T04:43:26Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:26Z- handleDepth()- Depth_Keller msg: 1730868206.06 epoch sec; seqNo:1210; depth 50.69 2024-11-06T04:43:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868206.09 epoch sec; seqNo:1210; 4061669.84 north; 594330.72 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T04:43:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868204.48 2024-11-06T04:43:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:26Z- waiting for 1730868208.48, time is 1730868206.09 2024-11-06T04:43:26Z- AHRS_M2 msg: 1730868206.46 epoch sec; seqNo:1113 2024-11-06T04:43:26Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:26Z- handleDepth()- Depth_Keller msg: 1730868206.47 epoch sec; seqNo:1211; depth 50.66 2024-11-06T04:43:26Z- handleDVL() - DVL msg epochMillisec: 1730868206.48 2024-11-06T04:43:26Z- handleDvl() - RDI_Pathfinder msg: 1730868206.48 sec; seqNo:401; #beams:4 2024-11-06T04:43:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.80 , 37.94 , 38.88 , 40.37 2024-11-06T04:43:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.01 2024-11-06T04:43:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868206.52 epoch sec; seqNo:1211; 4061669.86 north; 594331.94 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T04:43:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868204.48 2024-11-06T04:43:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:26Z- waiting for 1730868208.48, time is 1730868206.52 2024-11-06T04:43:26Z- AHRS_M2 msg: 1730868206.86 epoch sec; seqNo:1114 2024-11-06T04:43:26Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:26Z- handleDepth()- Depth_Keller msg: 1730868206.87 epoch sec; seqNo:1212; depth 50.65 2024-11-06T04:43:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868206.89 epoch sec; seqNo:1212; 4061669.86 north; 594331.94 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T04:43:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868204.48 2024-11-06T04:43:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:27Z- waiting for 1730868208.48, time is 1730868206.89 2024-11-06T04:43:27Z- AHRS_M2 msg: 1730868207.27 epoch sec; seqNo:1115 2024-11-06T04:43:27Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:27Z- handleDepth()- Depth_Keller msg: 1730868207.27 epoch sec; seqNo:1213; depth 50.65 2024-11-06T04:43:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868207.30 epoch sec; seqNo:1213; 4061669.86 north; 594331.94 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T04:43:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868204.48 2024-11-06T04:43:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:27Z- waiting for 1730868208.48, time is 1730868207.30 2024-11-06T04:43:27Z- AHRS_M2 msg: 1730868207.67 epoch sec; seqNo:1116 2024-11-06T04:43:27Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:27Z- handleDepth()- Depth_Keller msg: 1730868207.68 epoch sec; seqNo:1214; depth 50.62 2024-11-06T04:43:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868207.70 epoch sec; seqNo:1214; 4061669.87 north; 594333.16 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T04:43:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868204.48 2024-11-06T04:43:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:27Z- waiting for 1730868208.48, time is 1730868207.70 2024-11-06T04:43:28Z- AHRS_M2 msg: 1730868208.08 epoch sec; seqNo:1117 2024-11-06T04:43:28Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:28Z- handleDepth()- Depth_Keller msg: 1730868208.09 epoch sec; seqNo:1215; depth 50.62 2024-11-06T04:43:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868208.10 epoch sec; seqNo:1215; 4061670.25 north; 594333.15 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T04:43:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868204.48 2024-11-06T04:43:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:28Z- waiting for 1730868208.48, time is 1730868208.10 2024-11-06T04:43:28Z- AHRS_M2 msg: 1730868208.48 epoch sec; seqNo:1118 2024-11-06T04:43:28Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:28Z- handleDepth()- Depth_Keller msg: 1730868208.49 epoch sec; seqNo:1216; depth 50.58 2024-11-06T04:43:28Z- handleDVL() - DVL msg epochMillisec: 1730868208.51 2024-11-06T04:43:28Z- handleDvl() - RDI_Pathfinder msg: 1730868208.51 sec; seqNo:402; #beams:4 2024-11-06T04:43:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.43 , 38.32 , 38.88 , 40.56 2024-11-06T04:43:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , -0.01 2024-11-06T04:43:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868208.53 epoch sec; seqNo:1216; 4061670.25 north; 594333.15 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T04:43:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868204.48 2024-11-06T04:43:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:28Z- waiting for 1730868208.48, time is 1730868208.53 2024-11-06T04:43:28Z- LcmTrn::updateTrn() >>>> pose time 1730868208.53, meas time 1730868208.51 <<<< 2024-11-06T04:43:28Z- LcmTrn::updateTrn() equating pose 1730868208.51 and meas times 1730868208.51 2024-11-06T04:43:28Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868208.51 2024-11-06T04:43:28Z- TerrainNav::motionUpdate() - currEstimate = (4061670, 594333.15, 50.58) 2024-11-06T04:43:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:43:28Z- TerrainNav:: SubcloudNIS is 0.15, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:43:28Z- TNavPF::Projecting Measurements 2024-11-06T04:43:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:43:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:43:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:43:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:43:28Z- TNavFilter::4 good beams. 2024-11-06T04:43:28Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:43:28Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:43:28Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:43:28Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:43:28Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:43:28Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:43:28Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:43:28Z- TerrainMapOctree: (vehN, vehE ) = (4061670.25, 594333.15). 2024-11-06T04:43:28Z- TerrainMapOctree: Min Distance = 115.22. 2024-11-06T04:43:28Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:43:28Z- Forcing Subcloud Comparison 2024-11-06T04:43:28Z- Weighting particles with subcloud comparison 2024-11-06T04:43:28Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:28Z- meanExpectedMeasDiff: 1.721918 delta_rms_squared: 0.741262 alpha: 0.973728 2024-11-06T04:43:28Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:28Z- meanExpectedMeasDiff: 0.223146 delta_rms_squared: 0.744764 alpha: 0.973848 2024-11-06T04:43:28Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:28Z- meanExpectedMeasDiff: 1.675603 delta_rms_squared: 0.902866 alpha: 0.978328 2024-11-06T04:43:28Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:28Z- meanExpectedMeasDiff: -0.959887 delta_rms_squared: 0.947444 alpha: 0.979327 2024-11-06T04:43:28Z- Subcloud NIS: 0.164958 2024-11-06T04:43:28Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:43:28Z- TNavPF:: sumWeights = 0.385594 2024-11-06T04:43:28Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:43:28Z- TNavPF::First Term of Map Covariance Matrix is 0.47 2024-11-06T04:43:28Z- TNavPF::Calculated NIS Value : 0.24 numBeams normalized NIS: 0.06 2024-11-06T04:43:28Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868208.51 sec, ping # 0. 2024-11-06T04:43:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:43:28Z- variances([0], [2], and psi_berg) = 0.182865 0.187553 0.000000 2024-11-06T04:43:28Z- publishEstimatesDecoder() - at 1730868208.51 sec 2024-11-06T04:43:28Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:43:28Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061613.67 594337.56 54.86 2024-11-06T04:43:28Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061614.15 594337.22 55.16 lat/lon: 36.70 -121.94 2024-11-06T04:43:28Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.18 0.19 0.37 0.00 2024-11-06T04:43:28Z- AHRS_M2 msg: 1730868208.88 epoch sec; seqNo:1119 2024-11-06T04:43:28Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:28Z- handleDepth()- Depth_Keller msg: 1730868208.89 epoch sec; seqNo:1217; depth 50.57 2024-11-06T04:43:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868208.90 epoch sec; seqNo:1217; 4061670.26 north; 594334.37 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T04:43:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868208.51 2024-11-06T04:43:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:29Z- waiting for 1730868212.51, time is 1730868208.90 2024-11-06T04:43:29Z- AHRS_M2 msg: 1730868209.29 epoch sec; seqNo:1120 2024-11-06T04:43:29Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:29Z- handleDepth()- Depth_Keller msg: 1730868209.30 epoch sec; seqNo:1218; depth 50.56 2024-11-06T04:43:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868209.31 epoch sec; seqNo:1218; 4061670.26 north; 594334.37 east; 36.70 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T04:43:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868208.51 2024-11-06T04:43:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:29Z- waiting for 1730868212.51, time is 1730868209.31 2024-11-06T04:43:29Z- AHRS_M2 msg: 1730868209.69 epoch sec; seqNo:1121 2024-11-06T04:43:29Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:29Z- handleDepth()- Depth_Keller msg: 1730868209.70 epoch sec; seqNo:1219; depth 50.52 2024-11-06T04:43:29Z- handleDVL() - DVL msg epochMillisec: 1730868209.70 2024-11-06T04:43:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:29Z- handleDvl() - RDI_Pathfinder msg: 1730868209.70 sec; seqNo:403; #beams:0 2024-11-06T04:43:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , -0.01 2024-11-06T04:43:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868209.71 epoch sec; seqNo:1219; 4061670.26 north; 594334.37 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T04:43:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868208.51 2024-11-06T04:43:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:29Z- waiting for 1730868212.51, time is 1730868209.71 2024-11-06T04:43:30Z- AHRS_M2 msg: 1730868210.11 epoch sec; seqNo:1122 2024-11-06T04:43:30Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:30Z- handleDepth()- Depth_Keller msg: 1730868210.12 epoch sec; seqNo:1220; depth 50.50 2024-11-06T04:43:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868210.13 epoch sec; seqNo:1220; 4061670.26 north; 594334.37 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T04:43:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868208.51 2024-11-06T04:43:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:30Z- waiting for 1730868212.51, time is 1730868210.13 2024-11-06T04:43:30Z- AHRS_M2 msg: 1730868210.50 epoch sec; seqNo:1123 2024-11-06T04:43:30Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:30Z- handleDepth()- Depth_Keller msg: 1730868210.51 epoch sec; seqNo:1221; depth 50.47 2024-11-06T04:43:30Z- handleDVL() - DVL msg epochMillisec: 1730868210.53 2024-11-06T04:43:30Z- handleDvl() - RDI_Pathfinder msg: 1730868210.53 sec; seqNo:404; #beams:4 2024-11-06T04:43:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.05 , 38.13 , 38.69 , 40.56 2024-11-06T04:43:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , -0.06 2024-11-06T04:43:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868210.55 epoch sec; seqNo:1221; 4061670.28 north; 594335.59 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T04:43:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868208.51 2024-11-06T04:43:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:30Z- waiting for 1730868212.51, time is 1730868210.55 2024-11-06T04:43:30Z- AHRS_M2 msg: 1730868210.91 epoch sec; seqNo:1124 2024-11-06T04:43:30Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:30Z- handleDepth()- Depth_Keller msg: 1730868210.92 epoch sec; seqNo:1222; depth 50.46 2024-11-06T04:43:30Z- handleDVL() - DVL msg epochMillisec: 1730868210.92 2024-11-06T04:43:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:30Z- handleDvl() - RDI_Pathfinder msg: 1730868210.92 sec; seqNo:405; #beams:0 2024-11-06T04:43:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , -0.06 2024-11-06T04:43:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868210.93 epoch sec; seqNo:1222; 4061670.28 north; 594335.59 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T04:43:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868208.51 2024-11-06T04:43:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:31Z- waiting for 1730868212.51, time is 1730868210.93 2024-11-06T04:43:31Z- AHRS_M2 msg: 1730868211.32 epoch sec; seqNo:1125 2024-11-06T04:43:31Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:31Z- handleDepth()- Depth_Keller msg: 1730868211.33 epoch sec; seqNo:1223; depth 50.44 2024-11-06T04:43:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868211.34 epoch sec; seqNo:1223; 4061670.28 north; 594335.59 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T04:43:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868208.51 2024-11-06T04:43:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:31Z- waiting for 1730868212.51, time is 1730868211.34 2024-11-06T04:43:31Z- AHRS_M2 msg: 1730868211.72 epoch sec; seqNo:1126 2024-11-06T04:43:31Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:31Z- handleDepth()- Depth_Keller msg: 1730868211.73 epoch sec; seqNo:1224; depth 50.42 2024-11-06T04:43:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868211.75 epoch sec; seqNo:1224; 4061670.29 north; 594336.81 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T04:43:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868208.51 2024-11-06T04:43:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:31Z- waiting for 1730868212.51, time is 1730868211.75 2024-11-06T04:43:32Z- AHRS_M2 msg: 1730868212.12 epoch sec; seqNo:1127 2024-11-06T04:43:32Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:32Z- handleDepth()- Depth_Keller msg: 1730868212.13 epoch sec; seqNo:1225; depth 50.40 2024-11-06T04:43:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868212.14 epoch sec; seqNo:1225; 4061670.67 north; 594336.81 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T04:43:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868208.51 2024-11-06T04:43:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:32Z- waiting for 1730868212.51, time is 1730868212.14 2024-11-06T04:43:32Z- AHRS_M2 msg: 1730868212.53 epoch sec; seqNo:1128 2024-11-06T04:43:32Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:32Z- handleDepth()- Depth_Keller msg: 1730868212.54 epoch sec; seqNo:1226; depth 50.38 2024-11-06T04:43:32Z- handleDVL() - DVL msg epochMillisec: 1730868212.56 2024-11-06T04:43:32Z- handleDvl() - RDI_Pathfinder msg: 1730868212.56 sec; seqNo:406; #beams:4 2024-11-06T04:43:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.49 , 37.75 , 38.69 , 40.56 2024-11-06T04:43:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.04 , -0.08 2024-11-06T04:43:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868212.57 epoch sec; seqNo:1226; 4061670.67 north; 594336.81 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:43:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868208.51 2024-11-06T04:43:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:32Z- waiting for 1730868212.51, time is 1730868212.57 2024-11-06T04:43:32Z- LcmTrn::updateTrn() >>>> pose time 1730868212.57, meas time 1730868212.56 <<<< 2024-11-06T04:43:32Z- LcmTrn::updateTrn() equating pose 1730868212.56 and meas times 1730868212.56 2024-11-06T04:43:32Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868212.56 2024-11-06T04:43:32Z- TerrainNav::motionUpdate() - currEstimate = (4061671, 594336.81, 50.38) 2024-11-06T04:43:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:43:32Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:43:32Z- TNavPF::Projecting Measurements 2024-11-06T04:43:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:43:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:43:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:43:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:43:32Z- TNavFilter::4 good beams. 2024-11-06T04:43:32Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:43:32Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:43:32Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:43:32Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:43:32Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:43:32Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:43:32Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:43:32Z- TerrainMapOctree: (vehN, vehE ) = (4061670.67, 594336.81). 2024-11-06T04:43:32Z- TerrainMapOctree: Min Distance = 117.60. 2024-11-06T04:43:32Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:43:32Z- Forcing Subcloud Comparison 2024-11-06T04:43:32Z- Weighting particles with subcloud comparison 2024-11-06T04:43:32Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:32Z- meanExpectedMeasDiff: 1.861656 delta_rms_squared: 0.801399 alpha: 0.975651 2024-11-06T04:43:32Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:32Z- meanExpectedMeasDiff: -0.480305 delta_rms_squared: 1.018631 alpha: 0.980744 2024-11-06T04:43:32Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:32Z- meanExpectedMeasDiff: 1.203171 delta_rms_squared: 0.656467 alpha: 0.970435 2024-11-06T04:43:32Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:32Z- meanExpectedMeasDiff: -1.179815 delta_rms_squared: 1.265546 alpha: 0.984442 2024-11-06T04:43:32Z- Subcloud NIS: 0.174233 2024-11-06T04:43:32Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:43:32Z- TNavPF:: sumWeights = 0.358551 2024-11-06T04:43:32Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:43:32Z- TNavPF::First Term of Map Covariance Matrix is 0.59 2024-11-06T04:43:32Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.05 2024-11-06T04:43:32Z- TerrainNav::Resampling particle filter... 2024-11-06T04:43:32Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868212.56 sec, ping # 0. 2024-11-06T04:43:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:43:32Z- variances([0], [2], and psi_berg) = 0.186856 0.191522 0.000000 2024-11-06T04:43:32Z- publishEstimatesDecoder() - at 1730868212.56 sec 2024-11-06T04:43:32Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:43:32Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061614.73 594341.41 53.86 2024-11-06T04:43:32Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061614.51 594340.83 54.90 lat/lon: 36.70 -121.94 2024-11-06T04:43:32Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.19 0.19 0.37 0.00 2024-11-06T04:43:32Z- AHRS_M2 msg: 1730868212.93 epoch sec; seqNo:1129 2024-11-06T04:43:32Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:32Z- handleDepth()- Depth_Keller msg: 1730868212.94 epoch sec; seqNo:1227; depth 50.35 2024-11-06T04:43:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868212.99 epoch sec; seqNo:1227; 4061670.67 north; 594336.81 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:43:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868212.56 2024-11-06T04:43:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:33Z- waiting for 1730868216.56, time is 1730868212.99 2024-11-06T04:43:33Z- AHRS_M2 msg: 1730868213.34 epoch sec; seqNo:1130 2024-11-06T04:43:33Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:33Z- handleDepth()- Depth_Keller msg: 1730868213.35 epoch sec; seqNo:1228; depth 50.34 2024-11-06T04:43:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868213.36 epoch sec; seqNo:1228; 4061670.68 north; 594338.03 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:43:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868212.56 2024-11-06T04:43:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:33Z- waiting for 1730868216.56, time is 1730868213.36 2024-11-06T04:43:33Z- AHRS_M2 msg: 1730868213.74 epoch sec; seqNo:1131 2024-11-06T04:43:33Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:33Z- handleDepth()- Depth_Keller msg: 1730868213.75 epoch sec; seqNo:1229; depth 50.32 2024-11-06T04:43:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868213.77 epoch sec; seqNo:1229; 4061670.68 north; 594338.03 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:43:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868212.56 2024-11-06T04:43:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:33Z- waiting for 1730868216.56, time is 1730868213.77 2024-11-06T04:43:34Z- AHRS_M2 msg: 1730868214.14 epoch sec; seqNo:1132 2024-11-06T04:43:34Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.47 psi 2024-11-06T04:43:34Z- handleDepth()- Depth_Keller msg: 1730868214.15 epoch sec; seqNo:1230; depth 50.31 2024-11-06T04:43:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868214.16 epoch sec; seqNo:1230; 4061670.68 north; 594338.03 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:43:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868212.56 2024-11-06T04:43:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:34Z- waiting for 1730868216.56, time is 1730868214.16 2024-11-06T04:43:34Z- AHRS_M2 msg: 1730868214.55 epoch sec; seqNo:1133 2024-11-06T04:43:34Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.47 psi 2024-11-06T04:43:34Z- handleDepth()- Depth_Keller msg: 1730868214.55 epoch sec; seqNo:1231; depth 50.30 2024-11-06T04:43:34Z- handleDVL() - DVL msg epochMillisec: 1730868214.57 2024-11-06T04:43:34Z- handleDvl() - RDI_Pathfinder msg: 1730868214.57 sec; seqNo:407; #beams:4 2024-11-06T04:43:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.30 , 37.94 , 38.31 , 40.74 2024-11-06T04:43:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.07 , -0.04 2024-11-06T04:43:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868214.60 epoch sec; seqNo:1231; 4061670.68 north; 594338.03 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:43:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868212.56 2024-11-06T04:43:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:34Z- waiting for 1730868216.56, time is 1730868214.60 2024-11-06T04:43:34Z- AHRS_M2 msg: 1730868214.95 epoch sec; seqNo:1134 2024-11-06T04:43:34Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.47 psi 2024-11-06T04:43:34Z- handleDepth()- Depth_Keller msg: 1730868214.96 epoch sec; seqNo:1232; depth 50.28 2024-11-06T04:43:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868214.98 epoch sec; seqNo:1232; 4061670.70 north; 594339.25 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T04:43:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868212.56 2024-11-06T04:43:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:35Z- waiting for 1730868216.56, time is 1730868214.98 2024-11-06T04:43:35Z- AHRS_M2 msg: 1730868215.36 epoch sec; seqNo:1135 2024-11-06T04:43:35Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.47 psi 2024-11-06T04:43:35Z- handleDepth()- Depth_Keller msg: 1730868215.36 epoch sec; seqNo:1233; depth 50.28 2024-11-06T04:43:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868215.38 epoch sec; seqNo:1233; 4061670.70 north; 594339.25 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T04:43:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868212.56 2024-11-06T04:43:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:35Z- waiting for 1730868216.56, time is 1730868215.38 2024-11-06T04:43:35Z- AHRS_M2 msg: 1730868215.77 epoch sec; seqNo:1136 2024-11-06T04:43:35Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.47 psi 2024-11-06T04:43:35Z- handleDepth()- Depth_Keller msg: 1730868215.77 epoch sec; seqNo:1234; depth 50.27 2024-11-06T04:43:35Z- handleDVL() - DVL msg epochMillisec: 1730868215.78 2024-11-06T04:43:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:35Z- handleDvl() - RDI_Pathfinder msg: 1730868215.78 sec; seqNo:408; #beams:0 2024-11-06T04:43:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.07 , -0.04 2024-11-06T04:43:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868215.80 epoch sec; seqNo:1234; 4061671.07 north; 594339.25 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T04:43:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868212.56 2024-11-06T04:43:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:35Z- waiting for 1730868216.56, time is 1730868215.80 2024-11-06T04:43:36Z- AHRS_M2 msg: 1730868216.17 epoch sec; seqNo:1137 2024-11-06T04:43:36Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.46 psi 2024-11-06T04:43:36Z- handleDepth()- Depth_Keller msg: 1730868216.17 epoch sec; seqNo:1235; depth 50.26 2024-11-06T04:43:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868216.18 epoch sec; seqNo:1235; 4061671.07 north; 594339.25 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:43:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868212.56 2024-11-06T04:43:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:36Z- waiting for 1730868216.56, time is 1730868216.18 2024-11-06T04:43:36Z- AHRS_M2 msg: 1730868216.57 epoch sec; seqNo:1138 2024-11-06T04:43:36Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.46 psi 2024-11-06T04:43:36Z- handleDepth()- Depth_Keller msg: 1730868216.58 epoch sec; seqNo:1236; depth 50.26 2024-11-06T04:43:36Z- handleDVL() - DVL msg epochMillisec: 1730868216.60 2024-11-06T04:43:36Z- handleDvl() - RDI_Pathfinder msg: 1730868216.60 sec; seqNo:409; #beams:4 2024-11-06T04:43:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.49 , 37.94 , 38.13 , 40.56 2024-11-06T04:43:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.08 , 0.04 2024-11-06T04:43:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868216.61 epoch sec; seqNo:1236; 4061671.09 north; 594340.47 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:43:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868212.56 2024-11-06T04:43:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:36Z- waiting for 1730868216.56, time is 1730868216.61 2024-11-06T04:43:36Z- LcmTrn::updateTrn() >>>> pose time 1730868216.61, meas time 1730868216.60 <<<< 2024-11-06T04:43:36Z- LcmTrn::updateTrn() equating pose 1730868216.60 and meas times 1730868216.60 2024-11-06T04:43:36Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868216.60 2024-11-06T04:43:36Z- TerrainNav::motionUpdate() - currEstimate = (4061671, 594340.47, 50.26) 2024-11-06T04:43:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:43:36Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:43:36Z- TNavPF::Projecting Measurements 2024-11-06T04:43:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:43:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:43:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:43:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:43:36Z- TNavFilter::4 good beams. 2024-11-06T04:43:36Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:43:36Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:43:36Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:43:36Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:43:36Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:43:36Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:43:36Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:43:36Z- TerrainMapOctree: (vehN, vehE ) = (4061671.09, 594340.47). 2024-11-06T04:43:36Z- TerrainMapOctree: Min Distance = 120.05. 2024-11-06T04:43:36Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:43:36Z- Forcing Subcloud Comparison 2024-11-06T04:43:36Z- Weighting particles with subcloud comparison 2024-11-06T04:43:36Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:36Z- meanExpectedMeasDiff: 3.346255 delta_rms_squared: 0.802527 alpha: 0.975685 2024-11-06T04:43:36Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:36Z- meanExpectedMeasDiff: 0.305774 delta_rms_squared: 0.620856 alpha: 0.968792 2024-11-06T04:43:36Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:36Z- meanExpectedMeasDiff: 1.466598 delta_rms_squared: 0.686434 alpha: 0.971689 2024-11-06T04:43:36Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:36Z- meanExpectedMeasDiff: -0.506009 delta_rms_squared: 0.782216 alpha: 0.975069 2024-11-06T04:43:36Z- Subcloud NIS: 0.190629 2024-11-06T04:43:36Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:43:36Z- TNavPF:: sumWeights = 0.214563 2024-11-06T04:43:36Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:43:36Z- TNavPF::First Term of Map Covariance Matrix is 1.20 2024-11-06T04:43:36Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T04:43:36Z- TerrainNav::Resampling particle filter... 2024-11-06T04:43:36Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868216.60 sec, ping # 0. 2024-11-06T04:43:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:43:36Z- variances([0], [2], and psi_berg) = 0.206233 0.198898 0.000000 2024-11-06T04:43:36Z- publishEstimatesDecoder() - at 1730868216.60 sec 2024-11-06T04:43:36Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:43:36Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061615.19 594345.14 53.72 2024-11-06T04:43:36Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061614.89 594344.45 54.40 lat/lon: 36.70 -121.94 2024-11-06T04:43:36Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.20 0.40 0.00 2024-11-06T04:43:36Z- AHRS_M2 msg: 1730868216.97 epoch sec; seqNo:1139 2024-11-06T04:43:36Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.46 psi 2024-11-06T04:43:36Z- handleDepth()- Depth_Keller msg: 1730868216.98 epoch sec; seqNo:1237; depth 50.26 2024-11-06T04:43:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868216.99 epoch sec; seqNo:1237; 4061671.09 north; 594340.47 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:43:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868216.60 2024-11-06T04:43:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:37Z- waiting for 1730868220.60, time is 1730868216.99 2024-11-06T04:43:37Z- AHRS_M2 msg: 1730868217.38 epoch sec; seqNo:1140 2024-11-06T04:43:37Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.46 psi 2024-11-06T04:43:37Z- handleDepth()- Depth_Keller msg: 1730868217.38 epoch sec; seqNo:1238; depth 50.25 2024-11-06T04:43:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868217.39 epoch sec; seqNo:1238; 4061671.09 north; 594340.47 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T04:43:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868216.60 2024-11-06T04:43:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:37Z- waiting for 1730868220.60, time is 1730868217.39 2024-11-06T04:43:37Z- AHRS_M2 msg: 1730868217.78 epoch sec; seqNo:1141 2024-11-06T04:43:37Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.45 psi 2024-11-06T04:43:37Z- handleDepth()- Depth_Keller msg: 1730868217.79 epoch sec; seqNo:1239; depth 50.24 2024-11-06T04:43:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868217.81 epoch sec; seqNo:1239; 4061671.47 north; 594340.46 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:43:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868216.60 2024-11-06T04:43:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:37Z- waiting for 1730868220.60, time is 1730868217.81 2024-11-06T04:43:38Z- AHRS_M2 msg: 1730868218.18 epoch sec; seqNo:1142 2024-11-06T04:43:38Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.45 psi 2024-11-06T04:43:38Z- handleDepth()- Depth_Keller msg: 1730868218.19 epoch sec; seqNo:1240; depth 50.25 2024-11-06T04:43:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868218.20 epoch sec; seqNo:1240; 4061671.48 north; 594341.68 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T04:43:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868216.60 2024-11-06T04:43:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:38Z- waiting for 1730868220.60, time is 1730868218.20 2024-11-06T04:43:38Z- handleDepth()- Depth_Keller msg: 1730868218.60 epoch sec; seqNo:1241; depth 50.24 2024-11-06T04:43:38Z- handleDVL() - DVL msg epochMillisec: 1730868218.63 2024-11-06T04:43:38Z- handleDvl() - RDI_Pathfinder msg: 1730868218.63 sec; seqNo:410; #beams:4 2024-11-06T04:43:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.49 , 37.75 , 37.75 , 40.93 2024-11-06T04:43:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , 0.07 2024-11-06T04:43:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868218.64 epoch sec; seqNo:1241; 4061671.48 north; 594341.68 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:43:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868216.60 2024-11-06T04:43:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:38Z- waiting for 1730868220.60, time is 1730868218.64 2024-11-06T04:43:39Z- AHRS_M2 msg: 1730868218.99 epoch sec; seqNo:1143 2024-11-06T04:43:39Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.45 psi 2024-11-06T04:43:39Z- handleDepth()- Depth_Keller msg: 1730868219.00 epoch sec; seqNo:1242; depth 50.24 2024-11-06T04:43:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868219.01 epoch sec; seqNo:1242; 4061671.48 north; 594341.68 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:43:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868216.60 2024-11-06T04:43:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:39Z- waiting for 1730868220.60, time is 1730868219.01 2024-11-06T04:43:39Z- AHRS_M2 msg: 1730868219.39 epoch sec; seqNo:1144 2024-11-06T04:43:39Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.45 psi 2024-11-06T04:43:39Z- handleDepth()- Depth_Keller msg: 1730868219.40 epoch sec; seqNo:1243; depth 50.23 2024-11-06T04:43:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868219.41 epoch sec; seqNo:1243; 4061671.49 north; 594342.90 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T04:43:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868216.60 2024-11-06T04:43:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:39Z- waiting for 1730868220.60, time is 1730868219.41 2024-11-06T04:43:39Z- AHRS_M2 msg: 1730868219.80 epoch sec; seqNo:1145 2024-11-06T04:43:39Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.46 psi 2024-11-06T04:43:39Z- handleDepth()- Depth_Keller msg: 1730868219.81 epoch sec; seqNo:1244; depth 50.23 2024-11-06T04:43:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868219.84 epoch sec; seqNo:1244; 4061671.49 north; 594342.90 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T04:43:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868216.60 2024-11-06T04:43:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:39Z- waiting for 1730868220.60, time is 1730868219.84 2024-11-06T04:43:40Z- AHRS_M2 msg: 1730868220.20 epoch sec; seqNo:1146 2024-11-06T04:43:40Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.46 psi 2024-11-06T04:43:40Z- handleDepth()- Depth_Keller msg: 1730868220.21 epoch sec; seqNo:1245; depth 50.22 2024-11-06T04:43:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868220.22 epoch sec; seqNo:1245; 4061671.87 north; 594342.90 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T04:43:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868216.60 2024-11-06T04:43:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:40Z- waiting for 1730868220.60, time is 1730868220.22 2024-11-06T04:43:40Z- AHRS_M2 msg: 1730868220.61 epoch sec; seqNo:1147 2024-11-06T04:43:40Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.46 psi 2024-11-06T04:43:40Z- handleDepth()- Depth_Keller msg: 1730868220.62 epoch sec; seqNo:1246; depth 50.21 2024-11-06T04:43:40Z- handleDVL() - DVL msg epochMillisec: 1730868220.64 2024-11-06T04:43:40Z- handleDvl() - RDI_Pathfinder msg: 1730868220.64 sec; seqNo:411; #beams:4 2024-11-06T04:43:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.56 , 37.76 , 37.38 , 40.93 2024-11-06T04:43:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.03 2024-11-06T04:43:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868220.65 epoch sec; seqNo:1246; 4061671.87 north; 594342.90 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T04:43:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868216.60 2024-11-06T04:43:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:40Z- waiting for 1730868220.60, time is 1730868220.65 2024-11-06T04:43:40Z- LcmTrn::updateTrn() >>>> pose time 1730868220.65, meas time 1730868220.64 <<<< 2024-11-06T04:43:40Z- LcmTrn::updateTrn() equating pose 1730868220.64 and meas times 1730868220.64 2024-11-06T04:43:40Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868220.64 2024-11-06T04:43:40Z- TerrainNav::motionUpdate() - currEstimate = (4061672, 594342.90, 50.21) 2024-11-06T04:43:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:43:40Z- TerrainNav:: SubcloudNIS is 0.19, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:43:40Z- TNavPF::Projecting Measurements 2024-11-06T04:43:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:43:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:43:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:43:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:43:40Z- TNavFilter::4 good beams. 2024-11-06T04:43:40Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:43:40Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:43:40Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:43:40Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:43:40Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:43:40Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:43:40Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:43:40Z- TerrainMapOctree: (vehN, vehE ) = (4061671.87, 594342.90). 2024-11-06T04:43:40Z- TerrainMapOctree: Min Distance = 121.38. 2024-11-06T04:43:40Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:43:40Z- Forcing Subcloud Comparison 2024-11-06T04:43:40Z- Weighting particles with subcloud comparison 2024-11-06T04:43:40Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:40Z- meanExpectedMeasDiff: 2.970919 delta_rms_squared: 0.968638 alpha: 0.979770 2024-11-06T04:43:40Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:40Z- meanExpectedMeasDiff: -0.188712 delta_rms_squared: 0.741606 alpha: 0.973740 2024-11-06T04:43:40Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:40Z- meanExpectedMeasDiff: 1.272583 delta_rms_squared: 0.874628 alpha: 0.977644 2024-11-06T04:43:40Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:40Z- meanExpectedMeasDiff: -0.772424 delta_rms_squared: 0.848341 alpha: 0.976968 2024-11-06T04:43:40Z- Subcloud NIS: 0.204162 2024-11-06T04:43:40Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:43:40Z- TNavPF:: sumWeights = 0.245391 2024-11-06T04:43:40Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:43:40Z- TNavPF::First Term of Map Covariance Matrix is 1.05 2024-11-06T04:43:40Z- TNavPF::Calculated NIS Value : 0.14 numBeams normalized NIS: 0.03 2024-11-06T04:43:40Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868220.64 sec, ping # 0. 2024-11-06T04:43:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:43:40Z- variances([0], [2], and psi_berg) = 0.221211 0.194953 0.000000 2024-11-06T04:43:40Z- publishEstimatesDecoder() - at 1730868220.64 sec 2024-11-06T04:43:40Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:43:40Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061614.31 594346.17 54.68 2024-11-06T04:43:40Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061615.66 594346.89 54.02 lat/lon: 36.70 -121.94 2024-11-06T04:43:40Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.19 0.37 0.00 2024-11-06T04:43:41Z- AHRS_M2 msg: 1730868221.01 epoch sec; seqNo:1148 2024-11-06T04:43:41Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.46 psi 2024-11-06T04:43:41Z- handleDepth()- Depth_Keller msg: 1730868221.02 epoch sec; seqNo:1247; depth 50.21 2024-11-06T04:43:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868221.05 epoch sec; seqNo:1247; 4061671.89 north; 594344.12 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T04:43:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868220.64 2024-11-06T04:43:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:41Z- waiting for 1730868224.64, time is 1730868221.05 2024-11-06T04:43:41Z- AHRS_M2 msg: 1730868221.41 epoch sec; seqNo:1149 2024-11-06T04:43:41Z- AHRS_M2 msg: 0.16 phi; 0.03 theta; 1.47 psi 2024-11-06T04:43:41Z- handleDepth()- Depth_Keller msg: 1730868221.43 epoch sec; seqNo:1248; depth 50.20 2024-11-06T04:43:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868221.43 epoch sec; seqNo:1248; 4061671.89 north; 594344.12 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:43:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868220.64 2024-11-06T04:43:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:41Z- waiting for 1730868224.64, time is 1730868221.43 2024-11-06T04:43:41Z- AHRS_M2 msg: 1730868221.83 epoch sec; seqNo:1150 2024-11-06T04:43:41Z- AHRS_M2 msg: 0.16 phi; 0.03 theta; 1.47 psi 2024-11-06T04:43:41Z- handleDepth()- Depth_Keller msg: 1730868221.83 epoch sec; seqNo:1249; depth 50.17 2024-11-06T04:43:41Z- handleDVL() - DVL msg epochMillisec: 1730868221.84 2024-11-06T04:43:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:41Z- handleDvl() - RDI_Pathfinder msg: 1730868221.84 sec; seqNo:412; #beams:0 2024-11-06T04:43:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.03 2024-11-06T04:43:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868221.85 epoch sec; seqNo:1249; 4061671.89 north; 594344.12 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T04:43:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868220.64 2024-11-06T04:43:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:41Z- waiting for 1730868224.64, time is 1730868221.85 2024-11-06T04:43:42Z- AHRS_M2 msg: 1730868222.22 epoch sec; seqNo:1151 2024-11-06T04:43:42Z- AHRS_M2 msg: 0.16 phi; 0.03 theta; 1.47 psi 2024-11-06T04:43:42Z- handleDepth()- Depth_Keller msg: 1730868222.23 epoch sec; seqNo:1250; depth 50.17 2024-11-06T04:43:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868222.24 epoch sec; seqNo:1250; 4061671.90 north; 594345.34 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T04:43:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868220.64 2024-11-06T04:43:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:42Z- waiting for 1730868224.64, time is 1730868222.24 2024-11-06T04:43:42Z- AHRS_M2 msg: 1730868222.63 epoch sec; seqNo:1152 2024-11-06T04:43:42Z- AHRS_M2 msg: 0.16 phi; 0.03 theta; 1.47 psi 2024-11-06T04:43:42Z- handleDepth()- Depth_Keller msg: 1730868222.64 epoch sec; seqNo:1251; depth 50.14 2024-11-06T04:43:42Z- handleDVL() - DVL msg epochMillisec: 1730868222.66 2024-11-06T04:43:42Z- handleDvl() - RDI_Pathfinder msg: 1730868222.66 sec; seqNo:413; #beams:4 2024-11-06T04:43:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.81 , 37.75 , 37.57 , 40.93 2024-11-06T04:43:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , -0.05 2024-11-06T04:43:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868222.68 epoch sec; seqNo:1251; 4061671.90 north; 594345.34 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:43:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868220.64 2024-11-06T04:43:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:42Z- waiting for 1730868224.64, time is 1730868222.68 2024-11-06T04:43:43Z- AHRS_M2 msg: 1730868223.03 epoch sec; seqNo:1153 2024-11-06T04:43:43Z- AHRS_M2 msg: 0.16 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:43Z- handleDepth()- Depth_Keller msg: 1730868223.04 epoch sec; seqNo:1252; depth 50.14 2024-11-06T04:43:43Z- handleDVL() - DVL msg epochMillisec: 1730868223.04 2024-11-06T04:43:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:43Z- handleDvl() - RDI_Pathfinder msg: 1730868223.04 sec; seqNo:414; #beams:0 2024-11-06T04:43:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , -0.05 2024-11-06T04:43:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868223.06 epoch sec; seqNo:1252; 4061671.90 north; 594345.34 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:43:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868220.64 2024-11-06T04:43:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:43Z- waiting for 1730868224.64, time is 1730868223.06 2024-11-06T04:43:43Z- AHRS_M2 msg: 1730868223.44 epoch sec; seqNo:1154 2024-11-06T04:43:43Z- AHRS_M2 msg: 0.16 phi; 0.03 theta; 1.48 psi 2024-11-06T04:43:43Z- handleDepth()- Depth_Keller msg: 1730868223.44 epoch sec; seqNo:1253; depth 50.12 2024-11-06T04:43:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868223.45 epoch sec; seqNo:1253; 4061671.90 north; 594345.34 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T04:43:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868220.64 2024-11-06T04:43:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:43Z- waiting for 1730868224.64, time is 1730868223.45 2024-11-06T04:43:43Z- AHRS_M2 msg: 1730868223.84 epoch sec; seqNo:1155 2024-11-06T04:43:43Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:43Z- handleDepth()- Depth_Keller msg: 1730868223.85 epoch sec; seqNo:1254; depth 50.10 2024-11-06T04:43:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868223.87 epoch sec; seqNo:1254; 4061672.29 north; 594346.56 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T04:43:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868220.64 2024-11-06T04:43:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:44Z- waiting for 1730868224.64, time is 1730868223.87 2024-11-06T04:43:44Z- AHRS_M2 msg: 1730868224.24 epoch sec; seqNo:1156 2024-11-06T04:43:44Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:44Z- handleDepth()- Depth_Keller msg: 1730868224.25 epoch sec; seqNo:1255; depth 50.09 2024-11-06T04:43:44Z- handleDVL() - DVL msg epochMillisec: 1730868224.26 2024-11-06T04:43:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:44Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:44Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:44Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:44Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:44Z- handleDvl() - RDI_Pathfinder msg: 1730868224.26 sec; seqNo:415; #beams:0 2024-11-06T04:43:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , -0.05 2024-11-06T04:43:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868224.26 epoch sec; seqNo:1255; 4061672.29 north; 594346.56 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T04:43:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868220.64 2024-11-06T04:43:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:44Z- waiting for 1730868224.64, time is 1730868224.26 2024-11-06T04:43:44Z- AHRS_M2 msg: 1730868224.65 epoch sec; seqNo:1157 2024-11-06T04:43:44Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:44Z- handleDepth()- Depth_Keller msg: 1730868224.65 epoch sec; seqNo:1256; depth 50.06 2024-11-06T04:43:44Z- handleDVL() - DVL msg epochMillisec: 1730868224.67 2024-11-06T04:43:44Z- handleDvl() - RDI_Pathfinder msg: 1730868224.67 sec; seqNo:416; #beams:4 2024-11-06T04:43:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.37 , 37.75 , 37.38 , 40.56 2024-11-06T04:43:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , -0.08 2024-11-06T04:43:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868224.70 epoch sec; seqNo:1256; 4061672.29 north; 594346.56 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T04:43:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868220.64 2024-11-06T04:43:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:44Z- waiting for 1730868224.64, time is 1730868224.70 2024-11-06T04:43:44Z- LcmTrn::updateTrn() >>>> pose time 1730868224.70, meas time 1730868224.67 <<<< 2024-11-06T04:43:44Z- LcmTrn::updateTrn() equating pose 1730868224.67 and meas times 1730868224.67 2024-11-06T04:43:44Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868224.67 2024-11-06T04:43:44Z- TerrainNav::motionUpdate() - currEstimate = (4061672, 594346.56, 50.06) 2024-11-06T04:43:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:43:44Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:43:44Z- TNavPF::Projecting Measurements 2024-11-06T04:43:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:43:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:43:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:43:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:43:44Z- TNavFilter::4 good beams. 2024-11-06T04:43:44Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:43:44Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:43:44Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:43:44Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:43:44Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:43:44Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:43:44Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:43:44Z- TerrainMapOctree: (vehN, vehE ) = (4061672.29, 594346.56). 2024-11-06T04:43:44Z- TerrainMapOctree: Min Distance = 123.94. 2024-11-06T04:43:44Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:43:44Z- Forcing Subcloud Comparison 2024-11-06T04:43:44Z- Weighting particles with subcloud comparison 2024-11-06T04:43:44Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:44Z- meanExpectedMeasDiff: 4.151017 delta_rms_squared: 1.257836 alpha: 0.984349 2024-11-06T04:43:44Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:44Z- meanExpectedMeasDiff: 0.443946 delta_rms_squared: 0.706500 alpha: 0.972471 2024-11-06T04:43:44Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:44Z- meanExpectedMeasDiff: 0.971029 delta_rms_squared: 0.746535 alpha: 0.973909 2024-11-06T04:43:44Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:44Z- meanExpectedMeasDiff: -1.939525 delta_rms_squared: 0.821773 alpha: 0.976241 2024-11-06T04:43:44Z- Subcloud NIS: 0.226481 2024-11-06T04:43:44Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:43:44Z- TNavPF:: sumWeights = 0.079591 2024-11-06T04:43:44Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:43:44Z- TNavPF::First Term of Map Covariance Matrix is 0.86 2024-11-06T04:43:44Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T04:43:44Z- TerrainNav::Resampling particle filter... 2024-11-06T04:43:44Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868224.67 sec, ping # 0. 2024-11-06T04:43:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:43:44Z- variances([0], [2], and psi_berg) = 0.222970 0.210160 0.000000 2024-11-06T04:43:44Z- publishEstimatesDecoder() - at 1730868224.67 sec 2024-11-06T04:43:44Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:43:44Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061616.39 594351.29 53.32 2024-11-06T04:43:44Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061616.06 594350.38 53.52 lat/lon: 36.70 -121.94 2024-11-06T04:43:44Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.21 0.37 0.00 2024-11-06T04:43:45Z- AHRS_M2 msg: 1730868225.05 epoch sec; seqNo:1158 2024-11-06T04:43:45Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:45Z- handleDepth()- Depth_Keller msg: 1730868225.06 epoch sec; seqNo:1257; depth 50.04 2024-11-06T04:43:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868225.07 epoch sec; seqNo:1257; 4061672.30 north; 594347.78 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T04:43:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868224.67 2024-11-06T04:43:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:45Z- waiting for 1730868228.67, time is 1730868225.07 2024-11-06T04:43:45Z- AHRS_M2 msg: 1730868225.48 epoch sec; seqNo:1159 2024-11-06T04:43:45Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:45Z- handleDepth()- Depth_Keller msg: 1730868225.49 epoch sec; seqNo:1258; depth 50.04 2024-11-06T04:43:45Z- handleDVL() - DVL msg epochMillisec: 1730868225.49 2024-11-06T04:43:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:45Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:45Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:45Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:45Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:45Z- handleDvl() - RDI_Pathfinder msg: 1730868225.49 sec; seqNo:417; #beams:0 2024-11-06T04:43:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , -0.08 2024-11-06T04:43:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868225.50 epoch sec; seqNo:1258; 4061672.30 north; 594347.78 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T04:43:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868224.67 2024-11-06T04:43:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:45Z- waiting for 1730868228.67, time is 1730868225.50 2024-11-06T04:43:45Z- AHRS_M2 msg: 1730868225.86 epoch sec; seqNo:1160 2024-11-06T04:43:45Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:45Z- handleDepth()- Depth_Keller msg: 1730868225.87 epoch sec; seqNo:1259; depth 50.01 2024-11-06T04:43:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868225.90 epoch sec; seqNo:1259; 4061672.30 north; 594347.78 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:43:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868224.67 2024-11-06T04:43:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:46Z- waiting for 1730868228.67, time is 1730868225.90 2024-11-06T04:43:46Z- AHRS_M2 msg: 1730868226.26 epoch sec; seqNo:1161 2024-11-06T04:43:46Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:46Z- handleDepth()- Depth_Keller msg: 1730868226.27 epoch sec; seqNo:1260; depth 49.99 2024-11-06T04:43:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868226.28 epoch sec; seqNo:1260; 4061672.32 north; 594349.00 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T04:43:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868224.67 2024-11-06T04:43:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:46Z- waiting for 1730868228.67, time is 1730868226.28 2024-11-06T04:43:46Z- AHRS_M2 msg: 1730868226.67 epoch sec; seqNo:1162 2024-11-06T04:43:46Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:46Z- handleDepth()- Depth_Keller msg: 1730868226.67 epoch sec; seqNo:1261; depth 49.98 2024-11-06T04:43:46Z- handleDVL() - DVL msg epochMillisec: 1730868226.70 2024-11-06T04:43:46Z- handleDvl() - RDI_Pathfinder msg: 1730868226.70 sec; seqNo:418; #beams:4 2024-11-06T04:43:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.49 , 37.75 , 36.82 , 40.37 2024-11-06T04:43:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , -0.06 2024-11-06T04:43:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868226.71 epoch sec; seqNo:1261; 4061672.32 north; 594349.00 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T04:43:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868224.67 2024-11-06T04:43:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:46Z- waiting for 1730868228.67, time is 1730868226.71 2024-11-06T04:43:47Z- AHRS_M2 msg: 1730868227.07 epoch sec; seqNo:1163 2024-11-06T04:43:47Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:47Z- handleDepth()- Depth_Keller msg: 1730868227.08 epoch sec; seqNo:1262; depth 49.96 2024-11-06T04:43:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868227.10 epoch sec; seqNo:1262; 4061672.32 north; 594349.00 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:43:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868224.67 2024-11-06T04:43:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:47Z- waiting for 1730868228.67, time is 1730868227.10 2024-11-06T04:43:47Z- AHRS_M2 msg: 1730868227.47 epoch sec; seqNo:1164 2024-11-06T04:43:47Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:47Z- handleDepth()- Depth_Keller msg: 1730868227.48 epoch sec; seqNo:1263; depth 49.95 2024-11-06T04:43:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868227.49 epoch sec; seqNo:1263; 4061672.70 north; 594348.99 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T04:43:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868224.67 2024-11-06T04:43:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:47Z- waiting for 1730868228.67, time is 1730868227.49 2024-11-06T04:43:47Z- AHRS_M2 msg: 1730868227.89 epoch sec; seqNo:1165 2024-11-06T04:43:47Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:47Z- handleDepth()- Depth_Keller msg: 1730868227.89 epoch sec; seqNo:1264; depth 49.94 2024-11-06T04:43:47Z- handleDVL() - DVL msg epochMillisec: 1730868227.89 2024-11-06T04:43:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:47Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:47Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:47Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:47Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:47Z- handleDvl() - RDI_Pathfinder msg: 1730868227.89 sec; seqNo:419; #beams:0 2024-11-06T04:43:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , -0.06 2024-11-06T04:43:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868227.90 epoch sec; seqNo:1264; 4061672.71 north; 594350.21 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T04:43:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868224.67 2024-11-06T04:43:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:48Z- waiting for 1730868228.67, time is 1730868227.90 2024-11-06T04:43:48Z- AHRS_M2 msg: 1730868228.29 epoch sec; seqNo:1166 2024-11-06T04:43:48Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:48Z- handleDepth()- Depth_Keller msg: 1730868228.30 epoch sec; seqNo:1265; depth 49.93 2024-11-06T04:43:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868228.30 epoch sec; seqNo:1265; 4061672.71 north; 594350.21 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T04:43:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868224.67 2024-11-06T04:43:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:48Z- waiting for 1730868228.67, time is 1730868228.30 2024-11-06T04:43:48Z- AHRS_M2 msg: 1730868228.69 epoch sec; seqNo:1167 2024-11-06T04:43:48Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:48Z- handleDepth()- Depth_Keller msg: 1730868228.70 epoch sec; seqNo:1266; depth 49.92 2024-11-06T04:43:48Z- handleDVL() - DVL msg epochMillisec: 1730868228.72 2024-11-06T04:43:48Z- handleDvl() - RDI_Pathfinder msg: 1730868228.72 sec; seqNo:420; #beams:4 2024-11-06T04:43:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.93 , 37.01 , 36.63 , 40.37 2024-11-06T04:43:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.05 , -0.01 2024-11-06T04:43:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868228.74 epoch sec; seqNo:1266; 4061672.71 north; 594350.21 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:43:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868224.67 2024-11-06T04:43:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:48Z- waiting for 1730868228.67, time is 1730868228.74 2024-11-06T04:43:48Z- LcmTrn::updateTrn() >>>> pose time 1730868228.74, meas time 1730868228.72 <<<< 2024-11-06T04:43:48Z- LcmTrn::updateTrn() equating pose 1730868228.72 and meas times 1730868228.72 2024-11-06T04:43:48Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868228.72 2024-11-06T04:43:48Z- TerrainNav::motionUpdate() - currEstimate = (4061673, 594350.21, 49.92) 2024-11-06T04:43:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:43:48Z- TerrainNav:: SubcloudNIS is 0.23, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:43:48Z- TNavPF::Projecting Measurements 2024-11-06T04:43:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:43:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:43:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:43:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:43:48Z- TNavFilter::4 good beams. 2024-11-06T04:43:48Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:43:48Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:43:48Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:43:48Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:43:48Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:43:48Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:43:48Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:43:48Z- TerrainMapOctree: (vehN, vehE ) = (4061672.71, 594350.21). 2024-11-06T04:43:48Z- TerrainMapOctree: Min Distance = 126.56. 2024-11-06T04:43:48Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:43:48Z- Forcing Subcloud Comparison 2024-11-06T04:43:48Z- Weighting particles with subcloud comparison 2024-11-06T04:43:48Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:48Z- meanExpectedMeasDiff: 4.494027 delta_rms_squared: 1.001563 alpha: 0.980422 2024-11-06T04:43:48Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:48Z- meanExpectedMeasDiff: -0.805636 delta_rms_squared: 0.690134 alpha: 0.971836 2024-11-06T04:43:48Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:48Z- meanExpectedMeasDiff: 0.831341 delta_rms_squared: 0.779185 alpha: 0.974975 2024-11-06T04:43:48Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:48Z- meanExpectedMeasDiff: -2.590522 delta_rms_squared: 1.267781 alpha: 0.984469 2024-11-06T04:43:48Z- Subcloud NIS: 0.253058 2024-11-06T04:43:48Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:43:48Z- TNavPF:: sumWeights = 0.031791 2024-11-06T04:43:48Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:43:48Z- TNavPF::First Term of Map Covariance Matrix is 0.56 2024-11-06T04:43:48Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T04:43:48Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868228.72 sec, ping # 0. 2024-11-06T04:43:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:43:48Z- variances([0], [2], and psi_berg) = 0.220803 0.230704 0.000000 2024-11-06T04:43:48Z- publishEstimatesDecoder() - at 1730868228.72 sec 2024-11-06T04:43:48Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:43:48Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061616.86 594355.20 52.70 2024-11-06T04:43:48Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061616.49 594354.09 53.20 lat/lon: 36.70 -121.94 2024-11-06T04:43:48Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.23 0.34 0.00 2024-11-06T04:43:49Z- AHRS_M2 msg: 1730868229.09 epoch sec; seqNo:1168 2024-11-06T04:43:49Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:49Z- handleDepth()- Depth_Keller msg: 1730868229.10 epoch sec; seqNo:1267; depth 49.90 2024-11-06T04:43:49Z- handleDVL() - DVL msg epochMillisec: 1730868229.11 2024-11-06T04:43:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:49Z- handleDvl() - RDI_Pathfinder msg: 1730868229.11 sec; seqNo:421; #beams:0 2024-11-06T04:43:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.05 , -0.01 2024-11-06T04:43:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868229.13 epoch sec; seqNo:1267; 4061672.71 north; 594350.21 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:43:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868228.72 2024-11-06T04:43:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:49Z- waiting for 1730868232.72, time is 1730868229.13 2024-11-06T04:43:49Z- AHRS_M2 msg: 1730868229.49 epoch sec; seqNo:1169 2024-11-06T04:43:49Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:49Z- handleDepth()- Depth_Keller msg: 1730868229.51 epoch sec; seqNo:1268; depth 49.90 2024-11-06T04:43:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868229.52 epoch sec; seqNo:1268; 4061672.72 north; 594351.43 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:43:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868228.72 2024-11-06T04:43:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:49Z- waiting for 1730868232.72, time is 1730868229.52 2024-11-06T04:43:49Z- AHRS_M2 msg: 1730868229.91 epoch sec; seqNo:1170 2024-11-06T04:43:49Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:49Z- handleDepth()- Depth_Keller msg: 1730868229.91 epoch sec; seqNo:1269; depth 49.89 2024-11-06T04:43:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868229.92 epoch sec; seqNo:1269; 4061672.72 north; 594351.43 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:43:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868228.72 2024-11-06T04:43:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:50Z- waiting for 1730868232.72, time is 1730868229.92 2024-11-06T04:43:50Z- AHRS_M2 msg: 1730868230.30 epoch sec; seqNo:1171 2024-11-06T04:43:50Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:50Z- handleDepth()- Depth_Keller msg: 1730868230.31 epoch sec; seqNo:1270; depth 49.90 2024-11-06T04:43:50Z- handleDVL() - DVL msg epochMillisec: 1730868230.33 2024-11-06T04:43:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:50Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:50Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:50Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:50Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:50Z- handleDvl() - RDI_Pathfinder msg: 1730868230.33 sec; seqNo:422; #beams:0 2024-11-06T04:43:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.05 , -0.01 2024-11-06T04:43:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868230.34 epoch sec; seqNo:1270; 4061672.72 north; 594351.43 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:43:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868228.72 2024-11-06T04:43:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:50Z- waiting for 1730868232.72, time is 1730868230.34 2024-11-06T04:43:50Z- AHRS_M2 msg: 1730868230.71 epoch sec; seqNo:1172 2024-11-06T04:43:50Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:50Z- handleDepth()- Depth_Keller msg: 1730868230.71 epoch sec; seqNo:1271; depth 49.89 2024-11-06T04:43:50Z- handleDVL() - DVL msg epochMillisec: 1730868230.73 2024-11-06T04:43:50Z- handleDvl() - RDI_Pathfinder msg: 1730868230.73 sec; seqNo:423; #beams:4 2024-11-06T04:43:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.74 , 37.19 , 36.07 , 40.74 2024-11-06T04:43:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , 0.01 2024-11-06T04:43:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868230.75 epoch sec; seqNo:1271; 4061673.10 north; 594351.43 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:43:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868228.72 2024-11-06T04:43:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:50Z- waiting for 1730868232.72, time is 1730868230.75 2024-11-06T04:43:51Z- AHRS_M2 msg: 1730868231.11 epoch sec; seqNo:1173 2024-11-06T04:43:51Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:51Z- handleDepth()- Depth_Keller msg: 1730868231.12 epoch sec; seqNo:1272; depth 49.88 2024-11-06T04:43:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868231.14 epoch sec; seqNo:1272; 4061673.12 north; 594352.65 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T04:43:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868228.72 2024-11-06T04:43:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:51Z- waiting for 1730868232.72, time is 1730868231.14 2024-11-06T04:43:51Z- AHRS_M2 msg: 1730868231.52 epoch sec; seqNo:1174 2024-11-06T04:43:51Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:51Z- handleDepth()- Depth_Keller msg: 1730868231.53 epoch sec; seqNo:1273; depth 49.88 2024-11-06T04:43:51Z- handleDVL() - DVL msg epochMillisec: 1730868231.54 2024-11-06T04:43:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:51Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:51Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:51Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:51Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:51Z- handleDvl() - RDI_Pathfinder msg: 1730868231.54 sec; seqNo:424; #beams:0 2024-11-06T04:43:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , 0.01 2024-11-06T04:43:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868231.55 epoch sec; seqNo:1273; 4061673.12 north; 594352.65 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T04:43:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868228.72 2024-11-06T04:43:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:51Z- waiting for 1730868232.72, time is 1730868231.55 2024-11-06T04:43:51Z- AHRS_M2 msg: 1730868231.92 epoch sec; seqNo:1175 2024-11-06T04:43:51Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:51Z- handleDepth()- Depth_Keller msg: 1730868231.93 epoch sec; seqNo:1274; depth 49.87 2024-11-06T04:43:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868231.94 epoch sec; seqNo:1274; 4061673.12 north; 594352.65 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:43:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868228.72 2024-11-06T04:43:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:52Z- waiting for 1730868232.72, time is 1730868231.94 2024-11-06T04:43:52Z- AHRS_M2 msg: 1730868232.33 epoch sec; seqNo:1176 2024-11-06T04:43:52Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:52Z- handleDepth()- Depth_Keller msg: 1730868232.33 epoch sec; seqNo:1275; depth 49.88 2024-11-06T04:43:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868232.37 epoch sec; seqNo:1275; 4061673.12 north; 594352.65 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T04:43:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868228.72 2024-11-06T04:43:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:52Z- waiting for 1730868232.72, time is 1730868232.37 2024-11-06T04:43:52Z- AHRS_M2 msg: 1730868232.73 epoch sec; seqNo:1177 2024-11-06T04:43:52Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:52Z- handleDepth()- Depth_Keller msg: 1730868232.73 epoch sec; seqNo:1276; depth 49.87 2024-11-06T04:43:52Z- handleDVL() - DVL msg epochMillisec: 1730868232.76 2024-11-06T04:43:52Z- handleDvl() - RDI_Pathfinder msg: 1730868232.76 sec; seqNo:425; #beams:4 2024-11-06T04:43:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 37.01 , 35.70 , 40.56 2024-11-06T04:43:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.02 2024-11-06T04:43:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868232.77 epoch sec; seqNo:1276; 4061673.13 north; 594353.87 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:43:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868228.72 2024-11-06T04:43:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:52Z- waiting for 1730868232.72, time is 1730868232.77 2024-11-06T04:43:52Z- LcmTrn::updateTrn() >>>> pose time 1730868232.77, meas time 1730868232.76 <<<< 2024-11-06T04:43:52Z- LcmTrn::updateTrn() equating pose 1730868232.76 and meas times 1730868232.76 2024-11-06T04:43:52Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868232.76 2024-11-06T04:43:52Z- TerrainNav::motionUpdate() - currEstimate = (4061673, 594353.87, 49.87) 2024-11-06T04:43:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:43:52Z- TerrainNav:: SubcloudNIS is 0.25, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:43:52Z- TNavPF::Projecting Measurements 2024-11-06T04:43:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:43:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:43:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:43:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:43:52Z- TNavFilter::4 good beams. 2024-11-06T04:43:52Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:43:52Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:43:52Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:43:52Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:43:52Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:43:52Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:43:52Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:43:52Z- TerrainMapOctree: (vehN, vehE ) = (4061673.13, 594353.87). 2024-11-06T04:43:52Z- TerrainMapOctree: Min Distance = 129.22. 2024-11-06T04:43:52Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:43:53Z- Forcing Subcloud Comparison 2024-11-06T04:43:53Z- Weighting particles with subcloud comparison 2024-11-06T04:43:53Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:53Z- meanExpectedMeasDiff: 5.258520 delta_rms_squared: 0.954214 alpha: 0.979471 2024-11-06T04:43:53Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:53Z- meanExpectedMeasDiff: -0.810794 delta_rms_squared: 1.068094 alpha: 0.981619 2024-11-06T04:43:53Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:53Z- meanExpectedMeasDiff: 0.813506 delta_rms_squared: 0.804629 alpha: 0.975747 2024-11-06T04:43:53Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:53Z- meanExpectedMeasDiff: -1.668667 delta_rms_squared: 0.772443 alpha: 0.974762 2024-11-06T04:43:53Z- Subcloud NIS: 0.284186 2024-11-06T04:43:53Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:43:53Z- TNavPF:: sumWeights = 0.027093 2024-11-06T04:43:53Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:43:53Z- TNavPF::First Term of Map Covariance Matrix is 0.61 2024-11-06T04:43:53Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T04:43:53Z- TerrainNav::Resampling particle filter... 2024-11-06T04:43:53Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868232.76 sec, ping # 0. 2024-11-06T04:43:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:43:53Z- variances([0], [2], and psi_berg) = 0.224699 0.239985 0.000000 2024-11-06T04:43:53Z- publishEstimatesDecoder() - at 1730868232.76 sec 2024-11-06T04:43:53Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:43:53Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061617.17 594358.49 52.62 2024-11-06T04:43:53Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061616.93 594357.78 52.81 lat/lon: 36.70 -121.94 2024-11-06T04:43:53Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.24 0.33 0.00 2024-11-06T04:43:53Z- AHRS_M2 msg: 1730868233.13 epoch sec; seqNo:1178 2024-11-06T04:43:53Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:53Z- handleDepth()- Depth_Keller msg: 1730868233.14 epoch sec; seqNo:1277; depth 49.88 2024-11-06T04:43:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868233.15 epoch sec; seqNo:1277; 4061673.13 north; 594353.87 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T04:43:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868232.76 2024-11-06T04:43:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:53Z- waiting for 1730868236.76, time is 1730868233.15 2024-11-06T04:43:53Z- AHRS_M2 msg: 1730868233.54 epoch sec; seqNo:1179 2024-11-06T04:43:53Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:53Z- handleDepth()- Depth_Keller msg: 1730868233.54 epoch sec; seqNo:1278; depth 49.87 2024-11-06T04:43:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868233.55 epoch sec; seqNo:1278; 4061673.51 north; 594353.87 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:43:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868232.76 2024-11-06T04:43:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:53Z- waiting for 1730868236.76, time is 1730868233.55 2024-11-06T04:43:53Z- AHRS_M2 msg: 1730868233.94 epoch sec; seqNo:1180 2024-11-06T04:43:53Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.48 psi 2024-11-06T04:43:53Z- handleDepth()- Depth_Keller msg: 1730868233.95 epoch sec; seqNo:1279; depth 49.88 2024-11-06T04:43:53Z- handleDVL() - DVL msg epochMillisec: 1730868233.95 2024-11-06T04:43:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:53Z- handleDvl() - RDI_Pathfinder msg: 1730868233.95 sec; seqNo:426; #beams:0 2024-11-06T04:43:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.02 2024-11-06T04:43:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868233.98 epoch sec; seqNo:1279; 4061673.52 north; 594355.09 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T04:43:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868232.76 2024-11-06T04:43:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:54Z- waiting for 1730868236.76, time is 1730868233.98 2024-11-06T04:43:54Z- AHRS_M2 msg: 1730868234.38 epoch sec; seqNo:1181 2024-11-06T04:43:54Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.49 psi 2024-11-06T04:43:54Z- handleDepth()- Depth_Keller msg: 1730868234.41 epoch sec; seqNo:1280; depth 49.88 2024-11-06T04:43:54Z- handleDVL() - DVL msg epochMillisec: 1730868234.44 2024-11-06T04:43:54Z- handleDvl() - RDI_Pathfinder msg: 1730868234.44 sec; seqNo:427; #beams:4 2024-11-06T04:43:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.49 , 37.01 , 35.14 , 39.81 2024-11-06T04:43:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.01 2024-11-06T04:43:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868234.46 epoch sec; seqNo:1280; 4061673.52 north; 594355.09 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T04:43:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868232.76 2024-11-06T04:43:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:54Z- waiting for 1730868236.76, time is 1730868234.46 2024-11-06T04:43:54Z- AHRS_M2 msg: 1730868234.75 epoch sec; seqNo:1182 2024-11-06T04:43:54Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.49 psi 2024-11-06T04:43:54Z- handleDepth()- Depth_Keller msg: 1730868234.75 epoch sec; seqNo:1281; depth 49.88 2024-11-06T04:43:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868234.77 epoch sec; seqNo:1281; 4061673.52 north; 594355.09 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T04:43:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868232.76 2024-11-06T04:43:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:54Z- waiting for 1730868236.76, time is 1730868234.77 2024-11-06T04:43:55Z- AHRS_M2 msg: 1730868235.16 epoch sec; seqNo:1183 2024-11-06T04:43:55Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.49 psi 2024-11-06T04:43:55Z- handleDepth()- Depth_Keller msg: 1730868235.16 epoch sec; seqNo:1282; depth 49.87 2024-11-06T04:43:55Z- handleDVL() - DVL msg epochMillisec: 1730868235.16 2024-11-06T04:43:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:55Z- handleDvl() - RDI_Pathfinder msg: 1730868235.16 sec; seqNo:428; #beams:0 2024-11-06T04:43:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.01 2024-11-06T04:43:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868235.17 epoch sec; seqNo:1282; 4061673.52 north; 594355.09 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:43:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868232.76 2024-11-06T04:43:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:55Z- waiting for 1730868236.76, time is 1730868235.17 2024-11-06T04:43:55Z- AHRS_M2 msg: 1730868235.56 epoch sec; seqNo:1184 2024-11-06T04:43:55Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.49 psi 2024-11-06T04:43:55Z- handleDepth()- Depth_Keller msg: 1730868235.57 epoch sec; seqNo:1283; depth 49.87 2024-11-06T04:43:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868235.58 epoch sec; seqNo:1283; 4061673.54 north; 594356.31 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:43:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868232.76 2024-11-06T04:43:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:55Z- waiting for 1730868236.76, time is 1730868235.58 2024-11-06T04:43:55Z- AHRS_M2 msg: 1730868235.96 epoch sec; seqNo:1185 2024-11-06T04:43:55Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.49 psi 2024-11-06T04:43:55Z- handleDepth()- Depth_Keller msg: 1730868235.97 epoch sec; seqNo:1284; depth 49.86 2024-11-06T04:43:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868235.98 epoch sec; seqNo:1284; 4061673.54 north; 594356.31 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:43:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868232.76 2024-11-06T04:43:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:56Z- waiting for 1730868236.76, time is 1730868235.98 2024-11-06T04:43:56Z- AHRS_M2 msg: 1730868236.41 epoch sec; seqNo:1186 2024-11-06T04:43:56Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.49 psi 2024-11-06T04:43:56Z- handleDepth()- Depth_Keller msg: 1730868236.43 epoch sec; seqNo:1285; depth 49.86 2024-11-06T04:43:56Z- handleDVL() - DVL msg epochMillisec: 1730868236.45 2024-11-06T04:43:56Z- handleDvl() - RDI_Pathfinder msg: 1730868236.45 sec; seqNo:429; #beams:4 2024-11-06T04:43:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.49 , 37.19 , 34.76 , 38.13 2024-11-06T04:43:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , -0.02 2024-11-06T04:43:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868236.47 epoch sec; seqNo:1285; 4061673.54 north; 594356.31 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:43:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868232.76 2024-11-06T04:43:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:56Z- waiting for 1730868236.76, time is 1730868236.47 2024-11-06T04:43:56Z- AHRS_M2 msg: 1730868236.77 epoch sec; seqNo:1187 2024-11-06T04:43:56Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.50 psi 2024-11-06T04:43:56Z- handleDepth()- Depth_Keller msg: 1730868236.77 epoch sec; seqNo:1286; depth 49.86 2024-11-06T04:43:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868236.80 epoch sec; seqNo:1286; 4061673.55 north; 594357.53 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:43:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868232.76 2024-11-06T04:43:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:56Z- waiting for 1730868236.76, time is 1730868236.80 2024-11-06T04:43:56Z- LcmTrn::updateTrn() >>>> pose time 1730868236.80, meas time 1730868236.45 <<<< 2024-11-06T04:43:56Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868236.80 2024-11-06T04:43:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868236.45 sec, ping # 0; waiting for INS data more recent than 1730868232.76... 2024-11-06T04:43:56Z- TerrainNav::motionUpdate() - currEstimate = (4061674, 594357.53, 49.86) 2024-11-06T04:43:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:43:56Z- TerrainNav:: SubcloudNIS is 0.28, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:43:56Z- TNavPF::Projecting Measurements 2024-11-06T04:43:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:43:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:43:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:43:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:43:56Z- TNavFilter::4 good beams. 2024-11-06T04:43:56Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:43:56Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:43:56Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:43:56Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:43:56Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:43:56Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:43:56Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:43:56Z- TerrainMapOctree: (vehN, vehE ) = (4061673.51, 594357.22). 2024-11-06T04:43:56Z- TerrainMapOctree: Min Distance = 131.70. 2024-11-06T04:43:56Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:43:57Z- Forcing Subcloud Comparison 2024-11-06T04:43:57Z- Weighting particles with subcloud comparison 2024-11-06T04:43:57Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:57Z- meanExpectedMeasDiff: 4.134959 delta_rms_squared: 0.962888 alpha: 0.979652 2024-11-06T04:43:57Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:57Z- meanExpectedMeasDiff: -0.066432 delta_rms_squared: 0.652998 alpha: 0.970282 2024-11-06T04:43:57Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:57Z- meanExpectedMeasDiff: -0.477975 delta_rms_squared: 0.798344 alpha: 0.975560 2024-11-06T04:43:57Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:43:57Z- meanExpectedMeasDiff: -4.359496 delta_rms_squared: 1.358566 alpha: 0.985492 2024-11-06T04:43:57Z- Subcloud NIS: 0.324668 2024-11-06T04:43:57Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:43:57Z- TNavPF:: sumWeights = 0.012141 2024-11-06T04:43:57Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:43:57Z- TNavPF::First Term of Map Covariance Matrix is 0.23 2024-11-06T04:43:57Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:43:57Z- TerrainNav::Resampling particle filter... 2024-11-06T04:43:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868236.45 sec. 2024-11-06T04:43:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:43:57Z- variances([0], [2], and psi_berg) = 0.253678 0.276972 0.000000 2024-11-06T04:43:57Z- publishEstimatesDecoder() - at 1730868236.80 sec 2024-11-06T04:43:57Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:43:57Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061617.65 594362.17 53.75 2024-11-06T04:43:57Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061617.43 594361.60 52.96 lat/lon: 36.70 -121.94 2024-11-06T04:43:57Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.28 0.58 0.00 2024-11-06T04:43:57Z- handleDepth()- Depth_Keller msg: 1730868237.21 epoch sec; seqNo:1287; depth 49.83 2024-11-06T04:43:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868237.22 epoch sec; seqNo:1287; 4061673.55 north; 594357.53 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:43:57Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:43:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868236.80 2024-11-06T04:43:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:57Z- waiting for 1730868240.80, time is 1730868237.22 2024-11-06T04:43:57Z- AHRS_M2 msg: 1730868237.58 epoch sec; seqNo:1188 2024-11-06T04:43:57Z- AHRS_M2 msg: 0.17 phi; 0.01 theta; 1.50 psi 2024-11-06T04:43:57Z- handleDepth()- Depth_Keller msg: 1730868237.58 epoch sec; seqNo:1288; depth 49.85 2024-11-06T04:43:57Z- handleDVL() - DVL msg epochMillisec: 1730868237.58 2024-11-06T04:43:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:57Z- handleDvl() - RDI_Pathfinder msg: 1730868237.58 sec; seqNo:430; #beams:0 2024-11-06T04:43:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , -0.02 2024-11-06T04:43:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868237.59 epoch sec; seqNo:1288; 4061673.93 north; 594357.52 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:43:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868236.80 2024-11-06T04:43:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:57Z- waiting for 1730868240.80, time is 1730868237.59 2024-11-06T04:43:57Z- AHRS_M2 msg: 1730868237.98 epoch sec; seqNo:1189 2024-11-06T04:43:57Z- AHRS_M2 msg: 0.17 phi; 0.01 theta; 1.50 psi 2024-11-06T04:43:57Z- handleDepth()- Depth_Keller msg: 1730868237.99 epoch sec; seqNo:1289; depth 49.83 2024-11-06T04:43:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868238.00 epoch sec; seqNo:1289; 4061673.94 north; 594358.74 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:43:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868236.80 2024-11-06T04:43:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:58Z- waiting for 1730868240.80, time is 1730868238.00 2024-11-06T04:43:58Z- AHRS_M2 msg: 1730868238.42 epoch sec; seqNo:1190 2024-11-06T04:43:58Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.50 psi 2024-11-06T04:43:58Z- handleDepth()- Depth_Keller msg: 1730868238.43 epoch sec; seqNo:1290; depth 49.82 2024-11-06T04:43:58Z- handleDVL() - DVL msg epochMillisec: 1730868238.46 2024-11-06T04:43:58Z- handleDvl() - RDI_Pathfinder msg: 1730868238.46 sec; seqNo:431; #beams:4 2024-11-06T04:43:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 37.04 , 34.15 , 38.74 2024-11-06T04:43:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.03 2024-11-06T04:43:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868238.48 epoch sec; seqNo:1290; 4061673.94 north; 594358.74 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:43:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868236.80 2024-11-06T04:43:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:58Z- waiting for 1730868240.80, time is 1730868238.48 2024-11-06T04:43:58Z- AHRS_M2 msg: 1730868238.79 epoch sec; seqNo:1191 2024-11-06T04:43:58Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.50 psi 2024-11-06T04:43:58Z- handleDepth()- Depth_Keller msg: 1730868238.80 epoch sec; seqNo:1291; depth 49.82 2024-11-06T04:43:58Z- handleDVL() - DVL msg epochMillisec: 1730868238.80 2024-11-06T04:43:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:43:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:43:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:43:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:43:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:43:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:43:58Z- handleDvl() - RDI_Pathfinder msg: 1730868238.80 sec; seqNo:432; #beams:0 2024-11-06T04:43:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:43:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.03 2024-11-06T04:43:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868238.82 epoch sec; seqNo:1291; 4061673.94 north; 594358.74 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:43:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:43:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868236.80 2024-11-06T04:43:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:58Z- waiting for 1730868240.80, time is 1730868238.82 2024-11-06T04:43:59Z- AHRS_M2 msg: 1730868239.19 epoch sec; seqNo:1192 2024-11-06T04:43:59Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.50 psi 2024-11-06T04:43:59Z- handleDepth()- Depth_Keller msg: 1730868239.20 epoch sec; seqNo:1292; depth 49.82 2024-11-06T04:43:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868239.22 epoch sec; seqNo:1292; 4061673.94 north; 594358.74 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:43:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868236.80 2024-11-06T04:43:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:59Z- waiting for 1730868240.80, time is 1730868239.22 2024-11-06T04:43:59Z- AHRS_M2 msg: 1730868239.60 epoch sec; seqNo:1193 2024-11-06T04:43:59Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.50 psi 2024-11-06T04:43:59Z- handleDepth()- Depth_Keller msg: 1730868239.61 epoch sec; seqNo:1293; depth 49.81 2024-11-06T04:43:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868239.62 epoch sec; seqNo:1293; 4061673.95 north; 594359.96 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:43:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:43:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868236.80 2024-11-06T04:43:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:43:59Z- waiting for 1730868240.80, time is 1730868239.62 2024-11-06T04:44:00Z- AHRS_M2 msg: 1730868240.00 epoch sec; seqNo:1194 2024-11-06T04:44:00Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.50 psi 2024-11-06T04:44:00Z- handleDepth()- Depth_Keller msg: 1730868240.01 epoch sec; seqNo:1294; depth 49.79 2024-11-06T04:44:00Z- handleDVL() - DVL msg epochMillisec: 1730868240.01 2024-11-06T04:44:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:44:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:44:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:44:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:44:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:44:00Z- handleDvl() - RDI_Pathfinder msg: 1730868240.01 sec; seqNo:433; #beams:0 2024-11-06T04:44:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:44:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.03 2024-11-06T04:44:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868240.02 epoch sec; seqNo:1294; 4061673.95 north; 594359.96 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:44:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868236.80 2024-11-06T04:44:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:00Z- waiting for 1730868240.80, time is 1730868240.02 2024-11-06T04:44:00Z- AHRS_M2 msg: 1730868240.43 epoch sec; seqNo:1195 2024-11-06T04:44:00Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.50 psi 2024-11-06T04:44:00Z- handleDepth()- Depth_Keller msg: 1730868240.43 epoch sec; seqNo:1295; depth 49.78 2024-11-06T04:44:00Z- handleDVL() - DVL msg epochMillisec: 1730868240.45 2024-11-06T04:44:00Z- handleDvl() - RDI_Pathfinder msg: 1730868240.45 sec; seqNo:434; #beams:4 2024-11-06T04:44:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.78 , 36.70 , 33.47 , 39.76 2024-11-06T04:44:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.07 , -0.03 2024-11-06T04:44:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868240.52 epoch sec; seqNo:1295; 4061673.95 north; 594359.96 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:44:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868236.80 2024-11-06T04:44:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:00Z- waiting for 1730868240.80, time is 1730868240.52 2024-11-06T04:44:00Z- AHRS_M2 msg: 1730868240.81 epoch sec; seqNo:1196 2024-11-06T04:44:00Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.50 psi 2024-11-06T04:44:00Z- handleDepth()- Depth_Keller msg: 1730868240.82 epoch sec; seqNo:1296; depth 49.78 2024-11-06T04:44:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868240.85 epoch sec; seqNo:1296; 4061673.95 north; 594359.96 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:44:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868236.80 2024-11-06T04:44:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:00Z- waiting for 1730868240.80, time is 1730868240.85 2024-11-06T04:44:00Z- LcmTrn::updateTrn() >>>> pose time 1730868240.85, meas time 1730868240.45 <<<< 2024-11-06T04:44:00Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868240.85 2024-11-06T04:44:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868240.45 sec, ping # 0; waiting for INS data more recent than 1730868236.80... 2024-11-06T04:44:00Z- TerrainNav::motionUpdate() - currEstimate = (4061674, 594359.96, 49.78) 2024-11-06T04:44:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:44:00Z- TerrainNav:: SubcloudNIS is 0.32, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:44:00Z- TNavPF::Projecting Measurements 2024-11-06T04:44:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:44:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:44:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:44:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:44:00Z- TNavFilter::4 good beams. 2024-11-06T04:44:00Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:44:00Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:44:00Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:44:00Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:44:00Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:44:00Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:44:00Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:44:00Z- TerrainMapOctree: (vehN, vehE ) = (4061673.92, 594359.73). 2024-11-06T04:44:00Z- TerrainMapOctree: Min Distance = 133.52. 2024-11-06T04:44:00Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:44:01Z- Forcing Subcloud Comparison 2024-11-06T04:44:01Z- Weighting particles with subcloud comparison 2024-11-06T04:44:01Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:01Z- meanExpectedMeasDiff: 3.138805 delta_rms_squared: 0.716533 alpha: 0.972846 2024-11-06T04:44:01Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:01Z- meanExpectedMeasDiff: -0.563480 delta_rms_squared: 0.951427 alpha: 0.979412 2024-11-06T04:44:01Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:01Z- meanExpectedMeasDiff: -1.625983 delta_rms_squared: 1.153244 alpha: 0.982953 2024-11-06T04:44:01Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:01Z- meanExpectedMeasDiff: -1.765101 delta_rms_squared: 1.128134 alpha: 0.982580 2024-11-06T04:44:01Z- Subcloud NIS: 0.347603 2024-11-06T04:44:01Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:44:01Z- TNavPF:: sumWeights = 0.124976 2024-11-06T04:44:01Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:44:01Z- TNavPF::First Term of Map Covariance Matrix is 1.06 2024-11-06T04:44:01Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.01 2024-11-06T04:44:01Z- TerrainNav::Resampling particle filter... 2024-11-06T04:44:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868240.45 sec. 2024-11-06T04:44:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:44:01Z- variances([0], [2], and psi_berg) = 0.229780 0.277647 0.000000 2024-11-06T04:44:01Z- publishEstimatesDecoder() - at 1730868240.85 sec 2024-11-06T04:44:01Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:44:01Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061618.07 594364.65 53.58 2024-11-06T04:44:01Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061617.71 594364.04 53.19 lat/lon: 36.70 -121.94 2024-11-06T04:44:01Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.28 0.65 0.00 2024-11-06T04:44:01Z- AHRS_M2 msg: 1730868241.21 epoch sec; seqNo:1197 2024-11-06T04:44:01Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.50 psi 2024-11-06T04:44:01Z- handleDepth()- Depth_Keller msg: 1730868241.22 epoch sec; seqNo:1297; depth 49.77 2024-11-06T04:44:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868241.23 epoch sec; seqNo:1297; 4061674.35 north; 594361.18 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T04:44:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868240.85 2024-11-06T04:44:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:01Z- waiting for 1730868244.85, time is 1730868241.23 2024-11-06T04:44:01Z- AHRS_M2 msg: 1730868241.62 epoch sec; seqNo:1198 2024-11-06T04:44:01Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.50 psi 2024-11-06T04:44:01Z- handleDepth()- Depth_Keller msg: 1730868241.62 epoch sec; seqNo:1298; depth 49.76 2024-11-06T04:44:01Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:44:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868240.85 2024-11-06T04:44:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:01Z- waiting for 1730868244.85, time is 1730868241.62 2024-11-06T04:44:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868241.76 epoch sec; seqNo:1298; 4061674.35 north; 594361.18 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T04:44:01Z- 1 msgs in 200 ms and 2 handle calls 2024-11-06T04:44:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868240.85 2024-11-06T04:44:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:01Z- waiting for 1730868244.85, time is 1730868241.76 2024-11-06T04:44:02Z- AHRS_M2 msg: 1730868242.02 epoch sec; seqNo:1199 2024-11-06T04:44:02Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.50 psi 2024-11-06T04:44:02Z- handleDepth()- Depth_Keller msg: 1730868242.03 epoch sec; seqNo:1299; depth 49.74 2024-11-06T04:44:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868242.04 epoch sec; seqNo:1299; 4061674.35 north; 594361.18 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T04:44:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868240.85 2024-11-06T04:44:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:02Z- waiting for 1730868244.85, time is 1730868242.04 2024-11-06T04:44:02Z- AHRS_M2 msg: 1730868242.43 epoch sec; seqNo:1200 2024-11-06T04:44:02Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.49 psi 2024-11-06T04:44:02Z- handleDepth()- Depth_Keller msg: 1730868242.44 epoch sec; seqNo:1300; depth 49.74 2024-11-06T04:44:02Z- handleDVL() - DVL msg epochMillisec: 1730868242.47 2024-11-06T04:44:02Z- handleDvl() - RDI_Pathfinder msg: 1730868242.47 sec; seqNo:435; #beams:4 2024-11-06T04:44:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.27 , 36.70 , 32.62 , 40.27 2024-11-06T04:44:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.03 2024-11-06T04:44:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868242.50 epoch sec; seqNo:1300; 4061674.36 north; 594362.40 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T04:44:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868240.85 2024-11-06T04:44:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:02Z- waiting for 1730868244.85, time is 1730868242.50 2024-11-06T04:44:02Z- AHRS_M2 msg: 1730868242.83 epoch sec; seqNo:1201 2024-11-06T04:44:02Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.49 psi 2024-11-06T04:44:02Z- handleDepth()- Depth_Keller msg: 1730868242.84 epoch sec; seqNo:1301; depth 49.73 2024-11-06T04:44:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868242.86 epoch sec; seqNo:1301; 4061674.36 north; 594362.40 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T04:44:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868240.85 2024-11-06T04:44:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:02Z- waiting for 1730868244.85, time is 1730868242.86 2024-11-06T04:44:03Z- AHRS_M2 msg: 1730868243.24 epoch sec; seqNo:1202 2024-11-06T04:44:03Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.49 psi 2024-11-06T04:44:03Z- handleDepth()- Depth_Keller msg: 1730868243.25 epoch sec; seqNo:1302; depth 49.72 2024-11-06T04:44:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868243.26 epoch sec; seqNo:1302; 4061674.36 north; 594362.40 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:44:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868240.85 2024-11-06T04:44:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:03Z- waiting for 1730868244.85, time is 1730868243.26 2024-11-06T04:44:03Z- AHRS_M2 msg: 1730868243.64 epoch sec; seqNo:1203 2024-11-06T04:44:03Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.49 psi 2024-11-06T04:44:03Z- handleDepth()- Depth_Keller msg: 1730868243.64 epoch sec; seqNo:1303; depth 49.72 2024-11-06T04:44:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868243.65 epoch sec; seqNo:1303; 4061674.36 north; 594362.40 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:44:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868240.85 2024-11-06T04:44:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:03Z- waiting for 1730868244.85, time is 1730868243.65 2024-11-06T04:44:04Z- AHRS_M2 msg: 1730868244.04 epoch sec; seqNo:1204 2024-11-06T04:44:04Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.49 psi 2024-11-06T04:44:04Z- handleDepth()- Depth_Keller msg: 1730868244.07 epoch sec; seqNo:1304; depth 49.72 2024-11-06T04:44:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868244.07 epoch sec; seqNo:1304; 4061674.37 north; 594363.62 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:44:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868240.85 2024-11-06T04:44:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:04Z- waiting for 1730868244.85, time is 1730868244.07 2024-11-06T04:44:04Z- AHRS_M2 msg: 1730868244.44 epoch sec; seqNo:1205 2024-11-06T04:44:04Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.49 psi 2024-11-06T04:44:04Z- handleDepth()- Depth_Keller msg: 1730868244.45 epoch sec; seqNo:1305; depth 49.74 2024-11-06T04:44:04Z- handleDVL() - DVL msg epochMillisec: 1730868244.49 2024-11-06T04:44:04Z- handleDvl() - RDI_Pathfinder msg: 1730868244.49 sec; seqNo:436; #beams:4 2024-11-06T04:44:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.27 , 36.36 , 31.77 , 39.93 2024-11-06T04:44:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.02 , -0.02 2024-11-06T04:44:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868244.51 epoch sec; seqNo:1305; 4061674.37 north; 594363.62 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T04:44:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868240.85 2024-11-06T04:44:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:04Z- waiting for 1730868244.85, time is 1730868244.51 2024-11-06T04:44:04Z- AHRS_M2 msg: 1730868244.86 epoch sec; seqNo:1206 2024-11-06T04:44:04Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.49 psi 2024-11-06T04:44:04Z- handleDepth()- Depth_Keller msg: 1730868244.86 epoch sec; seqNo:1306; depth 49.73 2024-11-06T04:44:04Z- handleDVL() - DVL msg epochMillisec: 1730868244.86 2024-11-06T04:44:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:44:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:44:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:44:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:44:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:44:04Z- handleDvl() - RDI_Pathfinder msg: 1730868244.86 sec; seqNo:437; #beams:0 2024-11-06T04:44:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:44:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.02 , -0.02 2024-11-06T04:44:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868244.88 epoch sec; seqNo:1306; 4061674.75 north; 594363.62 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T04:44:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868240.85 2024-11-06T04:44:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:05Z- waiting for 1730868244.85, time is 1730868244.88 2024-11-06T04:44:05Z- LcmTrn::updateTrn() >>>> pose time 1730868244.88, meas time 1730868244.86 <<<< 2024-11-06T04:44:05Z- LcmTrn::updateTrn() equating pose 1730868244.86 and meas times 1730868244.86 2024-11-06T04:44:05Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868244.86 2024-11-06T04:44:05Z- TerrainNav::motionUpdate() - currEstimate = (4061675, 594363.62, 49.73) 2024-11-06T04:44:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:44:05Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:44:05Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:44:05Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:44:05Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:44:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:44:05Z- variances([0], [2], and psi_berg) = 0.231652 0.280152 0.000000 2024-11-06T04:44:05Z- publishEstimatesDecoder() - at 1730868244.86 sec 2024-11-06T04:44:05Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:44:05Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061618.84 594368.24 53.56 2024-11-06T04:44:05Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061618.51 594367.69 53.13 lat/lon: 36.70 -121.94 2024-11-06T04:44:05Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.23 0.28 0.92 0.00 2024-11-06T04:44:05Z- AHRS_M2 msg: 1730868245.27 epoch sec; seqNo:1207 2024-11-06T04:44:05Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T04:44:05Z- handleDepth()- Depth_Keller msg: 1730868245.27 epoch sec; seqNo:1307; depth 49.72 2024-11-06T04:44:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868245.28 epoch sec; seqNo:1307; 4061674.77 north; 594364.84 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:44:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868244.86 2024-11-06T04:44:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:05Z- waiting for 1730868248.86, time is 1730868245.28 2024-11-06T04:44:05Z- AHRS_M2 msg: 1730868245.65 epoch sec; seqNo:1208 2024-11-06T04:44:05Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T04:44:05Z- handleDepth()- Depth_Keller msg: 1730868245.67 epoch sec; seqNo:1308; depth 49.74 2024-11-06T04:44:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868245.68 epoch sec; seqNo:1308; 4061674.77 north; 594364.84 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T04:44:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868244.86 2024-11-06T04:44:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:05Z- waiting for 1730868248.86, time is 1730868245.68 2024-11-06T04:44:06Z- AHRS_M2 msg: 1730868246.06 epoch sec; seqNo:1209 2024-11-06T04:44:06Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T04:44:06Z- handleDepth()- Depth_Keller msg: 1730868246.06 epoch sec; seqNo:1309; depth 49.72 2024-11-06T04:44:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868246.08 epoch sec; seqNo:1309; 4061674.77 north; 594364.84 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:44:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868244.86 2024-11-06T04:44:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:06Z- waiting for 1730868248.86, time is 1730868246.08 2024-11-06T04:44:06Z- AHRS_M2 msg: 1730868246.46 epoch sec; seqNo:1210 2024-11-06T04:44:06Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.49 psi 2024-11-06T04:44:06Z- handleDepth()- Depth_Keller msg: 1730868246.47 epoch sec; seqNo:1310; depth 49.72 2024-11-06T04:44:06Z- handleDVL() - DVL msg epochMillisec: 1730868246.50 2024-11-06T04:44:06Z- handleDvl() - RDI_Pathfinder msg: 1730868246.50 sec; seqNo:438; #beams:4 2024-11-06T04:44:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.93 , 36.36 , 31.60 , 39.25 2024-11-06T04:44:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , -0.00 2024-11-06T04:44:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868246.51 epoch sec; seqNo:1310; 4061674.77 north; 594364.84 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:44:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868244.86 2024-11-06T04:44:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:06Z- waiting for 1730868248.86, time is 1730868246.51 2024-11-06T04:44:06Z- AHRS_M2 msg: 1730868246.87 epoch sec; seqNo:1211 2024-11-06T04:44:06Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.49 psi 2024-11-06T04:44:06Z- handleDepth()- Depth_Keller msg: 1730868246.87 epoch sec; seqNo:1311; depth 49.73 2024-11-06T04:44:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868246.89 epoch sec; seqNo:1311; 4061674.78 north; 594366.06 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T04:44:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868244.86 2024-11-06T04:44:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:07Z- waiting for 1730868248.86, time is 1730868246.89 2024-11-06T04:44:07Z- AHRS_M2 msg: 1730868247.27 epoch sec; seqNo:1212 2024-11-06T04:44:07Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.49 psi 2024-11-06T04:44:07Z- handleDepth()- Depth_Keller msg: 1730868247.28 epoch sec; seqNo:1312; depth 49.75 2024-11-06T04:44:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868247.31 epoch sec; seqNo:1312; 4061674.78 north; 594366.06 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:44:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868244.86 2024-11-06T04:44:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:07Z- waiting for 1730868248.86, time is 1730868247.31 2024-11-06T04:44:07Z- AHRS_M2 msg: 1730868247.67 epoch sec; seqNo:1213 2024-11-06T04:44:07Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.49 psi 2024-11-06T04:44:07Z- handleDepth()- Depth_Keller msg: 1730868247.68 epoch sec; seqNo:1313; depth 49.75 2024-11-06T04:44:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868247.69 epoch sec; seqNo:1313; 4061674.78 north; 594366.06 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:44:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868244.86 2024-11-06T04:44:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:07Z- waiting for 1730868248.86, time is 1730868247.69 2024-11-06T04:44:08Z- AHRS_M2 msg: 1730868248.08 epoch sec; seqNo:1214 2024-11-06T04:44:08Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.49 psi 2024-11-06T04:44:08Z- handleDepth()- Depth_Keller msg: 1730868248.09 epoch sec; seqNo:1314; depth 49.75 2024-11-06T04:44:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868248.10 epoch sec; seqNo:1314; 4061674.78 north; 594366.06 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:44:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868244.86 2024-11-06T04:44:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:08Z- waiting for 1730868248.86, time is 1730868248.10 2024-11-06T04:44:08Z- AHRS_M2 msg: 1730868248.49 epoch sec; seqNo:1215 2024-11-06T04:44:08Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:44:08Z- handleDepth()- Depth_Keller msg: 1730868248.51 epoch sec; seqNo:1315; depth 49.76 2024-11-06T04:44:08Z- handleDVL() - DVL msg epochMillisec: 1730868248.54 2024-11-06T04:44:08Z- handleDvl() - RDI_Pathfinder msg: 1730868248.54 sec; seqNo:439; #beams:4 2024-11-06T04:44:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.08 , 36.36 , 31.60 , 39.08 2024-11-06T04:44:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.02 2024-11-06T04:44:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868248.56 epoch sec; seqNo:1315; 4061674.79 north; 594367.28 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T04:44:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868244.86 2024-11-06T04:44:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:08Z- waiting for 1730868248.86, time is 1730868248.56 2024-11-06T04:44:08Z- AHRS_M2 msg: 1730868248.89 epoch sec; seqNo:1216 2024-11-06T04:44:08Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:44:08Z- handleDepth()- Depth_Keller msg: 1730868248.89 epoch sec; seqNo:1316; depth 49.75 2024-11-06T04:44:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868248.91 epoch sec; seqNo:1316; 4061675.17 north; 594367.27 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:44:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868244.86 2024-11-06T04:44:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:09Z- waiting for 1730868248.86, time is 1730868248.91 2024-11-06T04:44:09Z- LcmTrn::updateTrn() >>>> pose time 1730868248.91, meas time 1730868248.54 <<<< 2024-11-06T04:44:09Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868248.91 2024-11-06T04:44:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868248.54 sec, ping # 0; waiting for INS data more recent than 1730868244.86... 2024-11-06T04:44:09Z- TerrainNav::motionUpdate() - currEstimate = (4061675, 594367.27, 49.75) 2024-11-06T04:44:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:44:09Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:44:09Z- TNavPF::Projecting Measurements 2024-11-06T04:44:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:44:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:44:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:44:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:44:09Z- TNavFilter::4 good beams. 2024-11-06T04:44:09Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:44:09Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:44:09Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:44:09Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:44:09Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:44:09Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:44:09Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:44:09Z- TerrainMapOctree: (vehN, vehE ) = (4061675.13, 594366.93). 2024-11-06T04:44:09Z- TerrainMapOctree: Min Distance = 138.85. 2024-11-06T04:44:09Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:44:09Z- Forcing Subcloud Comparison 2024-11-06T04:44:09Z- Weighting particles with subcloud comparison 2024-11-06T04:44:09Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:09Z- meanExpectedMeasDiff: 2.749292 delta_rms_squared: 1.815963 alpha: 0.989107 2024-11-06T04:44:09Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:09Z- meanExpectedMeasDiff: 0.233919 delta_rms_squared: 1.402060 alpha: 0.985936 2024-11-06T04:44:09Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:09Z- meanExpectedMeasDiff: -2.658898 delta_rms_squared: 1.570918 alpha: 0.987429 2024-11-06T04:44:09Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:09Z- meanExpectedMeasDiff: -0.373532 delta_rms_squared: 2.231877 alpha: 0.991119 2024-11-06T04:44:09Z- Subcloud NIS: 0.364758 2024-11-06T04:44:09Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:44:09Z- TNavPF:: sumWeights = 0.083261 2024-11-06T04:44:09Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:44:09Z- TNavPF::First Term of Map Covariance Matrix is 0.62 2024-11-06T04:44:09Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T04:44:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868248.54 sec. 2024-11-06T04:44:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:44:09Z- variances([0], [2], and psi_berg) = 0.224742 0.276570 0.000000 2024-11-06T04:44:09Z- publishEstimatesDecoder() - at 1730868248.91 sec 2024-11-06T04:44:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:44:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061617.38 594370.39 53.72 2024-11-06T04:44:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061618.93 594371.36 53.23 lat/lon: 36.70 -121.94 2024-11-06T04:44:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.28 0.68 0.00 2024-11-06T04:44:09Z- AHRS_M2 msg: 1730868249.29 epoch sec; seqNo:1217 2024-11-06T04:44:09Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:44:09Z- handleDepth()- Depth_Keller msg: 1730868249.30 epoch sec; seqNo:1317; depth 49.76 2024-11-06T04:44:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868249.31 epoch sec; seqNo:1317; 4061675.17 north; 594367.27 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T04:44:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868248.91 2024-11-06T04:44:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:09Z- waiting for 1730868252.91, time is 1730868249.31 2024-11-06T04:44:09Z- AHRS_M2 msg: 1730868249.69 epoch sec; seqNo:1218 2024-11-06T04:44:09Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:44:09Z- handleDepth()- Depth_Keller msg: 1730868249.70 epoch sec; seqNo:1318; depth 49.76 2024-11-06T04:44:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868249.72 epoch sec; seqNo:1318; 4061675.19 north; 594368.49 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T04:44:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868248.91 2024-11-06T04:44:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:09Z- waiting for 1730868252.91, time is 1730868249.72 2024-11-06T04:44:10Z- AHRS_M2 msg: 1730868250.10 epoch sec; seqNo:1219 2024-11-06T04:44:10Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.49 psi 2024-11-06T04:44:10Z- handleDepth()- Depth_Keller msg: 1730868250.11 epoch sec; seqNo:1319; depth 49.76 2024-11-06T04:44:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868250.12 epoch sec; seqNo:1319; 4061675.19 north; 594368.49 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T04:44:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868248.91 2024-11-06T04:44:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:10Z- waiting for 1730868252.91, time is 1730868250.12 2024-11-06T04:44:10Z- AHRS_M2 msg: 1730868250.51 epoch sec; seqNo:1220 2024-11-06T04:44:10Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.49 psi 2024-11-06T04:44:10Z- handleDepth()- Depth_Keller msg: 1730868250.52 epoch sec; seqNo:1320; depth 49.77 2024-11-06T04:44:10Z- handleDVL() - DVL msg epochMillisec: 1730868250.54 2024-11-06T04:44:10Z- handleDvl() - RDI_Pathfinder msg: 1730868250.54 sec; seqNo:440; #beams:4 2024-11-06T04:44:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.74 , 36.19 , 30.92 , 39.76 2024-11-06T04:44:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.01 2024-11-06T04:44:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868250.56 epoch sec; seqNo:1320; 4061675.19 north; 594368.49 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T04:44:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868248.91 2024-11-06T04:44:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:10Z- waiting for 1730868252.91, time is 1730868250.56 2024-11-06T04:44:10Z- AHRS_M2 msg: 1730868250.91 epoch sec; seqNo:1221 2024-11-06T04:44:10Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.49 psi 2024-11-06T04:44:10Z- handleDepth()- Depth_Keller msg: 1730868250.92 epoch sec; seqNo:1321; depth 49.78 2024-11-06T04:44:10Z- handleDVL() - DVL msg epochMillisec: 1730868250.92 2024-11-06T04:44:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:44:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:44:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:44:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:44:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:44:10Z- handleDvl() - RDI_Pathfinder msg: 1730868250.92 sec; seqNo:441; #beams:0 2024-11-06T04:44:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:44:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.01 2024-11-06T04:44:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868250.94 epoch sec; seqNo:1321; 4061675.19 north; 594368.49 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:44:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868248.91 2024-11-06T04:44:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:11Z- waiting for 1730868252.91, time is 1730868250.94 2024-11-06T04:44:11Z- AHRS_M2 msg: 1730868251.32 epoch sec; seqNo:1222 2024-11-06T04:44:11Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.49 psi 2024-11-06T04:44:11Z- handleDepth()- Depth_Keller msg: 1730868251.32 epoch sec; seqNo:1322; depth 49.79 2024-11-06T04:44:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868251.33 epoch sec; seqNo:1322; 4061675.20 north; 594369.71 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:44:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868248.91 2024-11-06T04:44:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:11Z- waiting for 1730868252.91, time is 1730868251.33 2024-11-06T04:44:11Z- AHRS_M2 msg: 1730868251.72 epoch sec; seqNo:1223 2024-11-06T04:44:11Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.49 psi 2024-11-06T04:44:11Z- handleDepth()- Depth_Keller msg: 1730868251.73 epoch sec; seqNo:1323; depth 49.78 2024-11-06T04:44:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868251.73 epoch sec; seqNo:1323; 4061675.20 north; 594369.71 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:44:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868248.91 2024-11-06T04:44:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:11Z- waiting for 1730868252.91, time is 1730868251.73 2024-11-06T04:44:12Z- AHRS_M2 msg: 1730868252.12 epoch sec; seqNo:1224 2024-11-06T04:44:12Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:44:12Z- handleDepth()- Depth_Keller msg: 1730868252.13 epoch sec; seqNo:1324; depth 49.78 2024-11-06T04:44:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868252.14 epoch sec; seqNo:1324; 4061675.20 north; 594369.71 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:44:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868248.91 2024-11-06T04:44:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:12Z- waiting for 1730868252.91, time is 1730868252.14 2024-11-06T04:44:12Z- AHRS_M2 msg: 1730868252.53 epoch sec; seqNo:1225 2024-11-06T04:44:12Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:44:12Z- handleDepth()- Depth_Keller msg: 1730868252.53 epoch sec; seqNo:1325; depth 49.79 2024-11-06T04:44:12Z- handleDVL() - DVL msg epochMillisec: 1730868252.56 2024-11-06T04:44:12Z- handleDvl() - RDI_Pathfinder msg: 1730868252.56 sec; seqNo:442; #beams:4 2024-11-06T04:44:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.08 , 36.53 , 30.41 , 40.27 2024-11-06T04:44:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , -0.03 2024-11-06T04:44:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868252.57 epoch sec; seqNo:1325; 4061675.21 north; 594370.93 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:44:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868248.91 2024-11-06T04:44:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:12Z- waiting for 1730868252.91, time is 1730868252.57 2024-11-06T04:44:12Z- AHRS_M2 msg: 1730868252.93 epoch sec; seqNo:1226 2024-11-06T04:44:12Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:44:12Z- handleDepth()- Depth_Keller msg: 1730868252.94 epoch sec; seqNo:1326; depth 49.79 2024-11-06T04:44:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868252.97 epoch sec; seqNo:1326; 4061675.21 north; 594370.93 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:44:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868248.91 2024-11-06T04:44:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:13Z- waiting for 1730868252.91, time is 1730868252.97 2024-11-06T04:44:13Z- LcmTrn::updateTrn() >>>> pose time 1730868252.97, meas time 1730868252.56 <<<< 2024-11-06T04:44:13Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868252.97 2024-11-06T04:44:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868252.56 sec, ping # 0; waiting for INS data more recent than 1730868248.91... 2024-11-06T04:44:13Z- TerrainNav::motionUpdate() - currEstimate = (4061675, 594370.93, 49.79) 2024-11-06T04:44:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:44:13Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:44:13Z- TNavPF::Projecting Measurements 2024-11-06T04:44:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:44:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:44:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:44:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:44:13Z- TNavFilter::4 good beams. 2024-11-06T04:44:13Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:44:13Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:44:13Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:44:13Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:44:13Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:44:13Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:44:13Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:44:13Z- TerrainMapOctree: (vehN, vehE ) = (4061675.21, 594370.56). 2024-11-06T04:44:13Z- TerrainMapOctree: Min Distance = 141.88. 2024-11-06T04:44:13Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:44:13Z- Forcing Subcloud Comparison 2024-11-06T04:44:13Z- Weighting particles with subcloud comparison 2024-11-06T04:44:13Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:13Z- meanExpectedMeasDiff: 2.253990 delta_rms_squared: 1.128005 alpha: 0.982578 2024-11-06T04:44:13Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:13Z- meanExpectedMeasDiff: 0.683123 delta_rms_squared: 1.055766 alpha: 0.981409 2024-11-06T04:44:13Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:13Z- meanExpectedMeasDiff: -3.552727 delta_rms_squared: 1.224770 alpha: 0.983933 2024-11-06T04:44:13Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:13Z- meanExpectedMeasDiff: 0.872585 delta_rms_squared: 1.177104 alpha: 0.983293 2024-11-06T04:44:13Z- Subcloud NIS: 0.389868 2024-11-06T04:44:13Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:44:13Z- TNavPF:: sumWeights = 0.047831 2024-11-06T04:44:13Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:44:13Z- TNavPF::First Term of Map Covariance Matrix is 0.27 2024-11-06T04:44:13Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T04:44:13Z- TerrainNav::Resampling particle filter... 2024-11-06T04:44:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868252.56 sec. 2024-11-06T04:44:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:44:13Z- variances([0], [2], and psi_berg) = 0.210167 0.288313 0.000000 2024-11-06T04:44:13Z- publishEstimatesDecoder() - at 1730868252.97 sec 2024-11-06T04:44:13Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:44:13Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061619.35 594375.50 53.71 2024-11-06T04:44:13Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061618.91 594375.10 53.43 lat/lon: 36.70 -121.94 2024-11-06T04:44:13Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.29 0.72 0.00 2024-11-06T04:44:13Z- AHRS_M2 msg: 1730868253.34 epoch sec; seqNo:1227 2024-11-06T04:44:13Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.50 psi 2024-11-06T04:44:13Z- handleDepth()- Depth_Keller msg: 1730868253.34 epoch sec; seqNo:1327; depth 49.78 2024-11-06T04:44:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868253.36 epoch sec; seqNo:1327; 4061675.59 north; 594370.93 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:44:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868252.97 2024-11-06T04:44:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:13Z- waiting for 1730868256.97, time is 1730868253.36 2024-11-06T04:44:13Z- AHRS_M2 msg: 1730868253.74 epoch sec; seqNo:1228 2024-11-06T04:44:13Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:44:13Z- handleDepth()- Depth_Keller msg: 1730868253.75 epoch sec; seqNo:1328; depth 49.81 2024-11-06T04:44:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868253.76 epoch sec; seqNo:1328; 4061675.59 north; 594370.93 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:44:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868252.97 2024-11-06T04:44:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:13Z- waiting for 1730868256.97, time is 1730868253.76 2024-11-06T04:44:14Z- AHRS_M2 msg: 1730868254.14 epoch sec; seqNo:1229 2024-11-06T04:44:14Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:44:14Z- handleDepth()- Depth_Keller msg: 1730868254.15 epoch sec; seqNo:1329; depth 49.80 2024-11-06T04:44:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868254.16 epoch sec; seqNo:1329; 4061675.60 north; 594372.15 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T04:44:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868252.97 2024-11-06T04:44:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:14Z- waiting for 1730868256.97, time is 1730868254.16 2024-11-06T04:44:14Z- AHRS_M2 msg: 1730868254.55 epoch sec; seqNo:1230 2024-11-06T04:44:14Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:44:14Z- handleDepth()- Depth_Keller msg: 1730868254.56 epoch sec; seqNo:1330; depth 49.78 2024-11-06T04:44:14Z- handleDVL() - DVL msg epochMillisec: 1730868254.58 2024-11-06T04:44:14Z- handleDvl() - RDI_Pathfinder msg: 1730868254.58 sec; seqNo:443; #beams:4 2024-11-06T04:44:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.40 , 35.68 , 30.41 , 39.76 2024-11-06T04:44:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.06 2024-11-06T04:44:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868254.59 epoch sec; seqNo:1330; 4061675.60 north; 594372.15 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:44:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868252.97 2024-11-06T04:44:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:14Z- waiting for 1730868256.97, time is 1730868254.59 2024-11-06T04:44:14Z- AHRS_M2 msg: 1730868254.95 epoch sec; seqNo:1231 2024-11-06T04:44:14Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:44:14Z- handleDepth()- Depth_Keller msg: 1730868254.96 epoch sec; seqNo:1331; depth 49.81 2024-11-06T04:44:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868254.98 epoch sec; seqNo:1331; 4061675.60 north; 594372.15 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:44:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868252.97 2024-11-06T04:44:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:15Z- waiting for 1730868256.97, time is 1730868254.98 2024-11-06T04:44:15Z- AHRS_M2 msg: 1730868255.36 epoch sec; seqNo:1232 2024-11-06T04:44:15Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:44:15Z- handleDepth()- Depth_Keller msg: 1730868255.36 epoch sec; seqNo:1332; depth 49.82 2024-11-06T04:44:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868255.37 epoch sec; seqNo:1332; 4061675.62 north; 594373.37 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:44:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868252.97 2024-11-06T04:44:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:15Z- waiting for 1730868256.97, time is 1730868255.37 2024-11-06T04:44:15Z- handleDepth()- Depth_Keller msg: 1730868255.78 epoch sec; seqNo:1333; depth 49.81 2024-11-06T04:44:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868255.79 epoch sec; seqNo:1333; 4061675.62 north; 594373.37 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:44:15Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:44:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868252.97 2024-11-06T04:44:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:15Z- waiting for 1730868256.97, time is 1730868255.79 2024-11-06T04:44:16Z- AHRS_M2 msg: 1730868256.16 epoch sec; seqNo:1233 2024-11-06T04:44:16Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:44:16Z- handleDepth()- Depth_Keller msg: 1730868256.17 epoch sec; seqNo:1334; depth 49.81 2024-11-06T04:44:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868256.19 epoch sec; seqNo:1334; 4061675.62 north; 594373.37 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:44:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868252.97 2024-11-06T04:44:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:16Z- waiting for 1730868256.97, time is 1730868256.19 2024-11-06T04:44:16Z- AHRS_M2 msg: 1730868256.57 epoch sec; seqNo:1234 2024-11-06T04:44:16Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:44:16Z- handleDepth()- Depth_Keller msg: 1730868256.58 epoch sec; seqNo:1335; depth 49.81 2024-11-06T04:44:16Z- handleDVL() - DVL msg epochMillisec: 1730868256.60 2024-11-06T04:44:16Z- handleDvl() - RDI_Pathfinder msg: 1730868256.60 sec; seqNo:444; #beams:4 2024-11-06T04:44:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.55 , 35.00 , 30.24 , 39.93 2024-11-06T04:44:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.04 , -0.04 2024-11-06T04:44:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868256.62 epoch sec; seqNo:1335; 4061675.62 north; 594373.37 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:44:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868252.97 2024-11-06T04:44:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:16Z- waiting for 1730868256.97, time is 1730868256.62 2024-11-06T04:44:16Z- AHRS_M2 msg: 1730868256.97 epoch sec; seqNo:1235 2024-11-06T04:44:16Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:44:16Z- handleDepth()- Depth_Keller msg: 1730868256.98 epoch sec; seqNo:1336; depth 49.84 2024-11-06T04:44:16Z- handleDVL() - DVL msg epochMillisec: 1730868256.98 2024-11-06T04:44:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:44:16Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:44:16Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:44:16Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:44:16Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:44:16Z- handleDvl() - RDI_Pathfinder msg: 1730868256.98 sec; seqNo:445; #beams:0 2024-11-06T04:44:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:44:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.04 , -0.04 2024-11-06T04:44:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868257.00 epoch sec; seqNo:1336; 4061675.63 north; 594374.59 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T04:44:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868252.97 2024-11-06T04:44:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:17Z- waiting for 1730868256.97, time is 1730868257.00 2024-11-06T04:44:17Z- LcmTrn::updateTrn() >>>> pose time 1730868257.00, meas time 1730868256.98 <<<< 2024-11-06T04:44:17Z- LcmTrn::updateTrn() equating pose 1730868256.98 and meas times 1730868256.98 2024-11-06T04:44:17Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868256.98 2024-11-06T04:44:17Z- TerrainNav::motionUpdate() - currEstimate = (4061676, 594374.59, 49.84) 2024-11-06T04:44:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:44:17Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:44:17Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:44:17Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:44:17Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:44:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:44:17Z- variances([0], [2], and psi_berg) = 0.212587 0.290730 0.000000 2024-11-06T04:44:17Z- publishEstimatesDecoder() - at 1730868256.98 sec 2024-11-06T04:44:17Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:44:17Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061619.78 594379.20 53.24 2024-11-06T04:44:17Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061619.32 594378.76 53.48 lat/lon: 36.70 -121.94 2024-11-06T04:44:17Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.29 0.95 0.00 2024-11-06T04:44:17Z- AHRS_M2 msg: 1730868257.37 epoch sec; seqNo:1236 2024-11-06T04:44:17Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.49 psi 2024-11-06T04:44:17Z- handleDepth()- Depth_Keller msg: 1730868257.38 epoch sec; seqNo:1337; depth 49.84 2024-11-06T04:44:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868257.40 epoch sec; seqNo:1337; 4061675.63 north; 594374.59 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T04:44:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868256.98 2024-11-06T04:44:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:17Z- waiting for 1730868260.98, time is 1730868257.40 2024-11-06T04:44:17Z- AHRS_M2 msg: 1730868257.78 epoch sec; seqNo:1237 2024-11-06T04:44:17Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.49 psi 2024-11-06T04:44:17Z- handleDepth()- Depth_Keller msg: 1730868257.79 epoch sec; seqNo:1338; depth 49.84 2024-11-06T04:44:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868257.80 epoch sec; seqNo:1338; 4061676.01 north; 594374.59 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T04:44:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868256.98 2024-11-06T04:44:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:17Z- waiting for 1730868260.98, time is 1730868257.80 2024-11-06T04:44:18Z- AHRS_M2 msg: 1730868258.19 epoch sec; seqNo:1238 2024-11-06T04:44:18Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:44:18Z- handleDepth()- Depth_Keller msg: 1730868258.19 epoch sec; seqNo:1339; depth 49.86 2024-11-06T04:44:18Z- handleDVL() - DVL msg epochMillisec: 1730868258.20 2024-11-06T04:44:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:44:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:44:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:44:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:44:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:44:18Z- handleDvl() - RDI_Pathfinder msg: 1730868258.20 sec; seqNo:446; #beams:0 2024-11-06T04:44:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:44:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.04 , -0.04 2024-11-06T04:44:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868258.26 epoch sec; seqNo:1339; 4061676.02 north; 594375.81 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:44:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868256.98 2024-11-06T04:44:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:18Z- waiting for 1730868260.98, time is 1730868258.26 2024-11-06T04:44:18Z- AHRS_M2 msg: 1730868258.60 epoch sec; seqNo:1239 2024-11-06T04:44:18Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:44:18Z- handleDepth()- Depth_Keller msg: 1730868258.61 epoch sec; seqNo:1340; depth 49.86 2024-11-06T04:44:18Z- handleDVL() - DVL msg epochMillisec: 1730868258.62 2024-11-06T04:44:18Z- handleDvl() - RDI_Pathfinder msg: 1730868258.62 sec; seqNo:447; #beams:4 2024-11-06T04:44:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.21 , 34.66 , 29.73 , 39.59 2024-11-06T04:44:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , -0.00 2024-11-06T04:44:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868258.65 epoch sec; seqNo:1340; 4061676.02 north; 594375.81 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:44:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868256.98 2024-11-06T04:44:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:18Z- waiting for 1730868260.98, time is 1730868258.65 2024-11-06T04:44:18Z- AHRS_M2 msg: 1730868258.99 epoch sec; seqNo:1240 2024-11-06T04:44:18Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.49 psi 2024-11-06T04:44:18Z- handleDepth()- Depth_Keller msg: 1730868259.00 epoch sec; seqNo:1341; depth 49.88 2024-11-06T04:44:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868259.03 epoch sec; seqNo:1341; 4061676.02 north; 594375.81 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T04:44:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868256.98 2024-11-06T04:44:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:19Z- waiting for 1730868260.98, time is 1730868259.03 2024-11-06T04:44:19Z- AHRS_M2 msg: 1730868259.40 epoch sec; seqNo:1241 2024-11-06T04:44:19Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:44:19Z- handleDepth()- Depth_Keller msg: 1730868259.40 epoch sec; seqNo:1342; depth 49.90 2024-11-06T04:44:19Z- handleDVL() - DVL msg epochMillisec: 1730868259.43 2024-11-06T04:44:19Z- handleDvl() - RDI_Pathfinder msg: 1730868259.43 sec; seqNo:448; #beams:4 2024-11-06T04:44:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.04 , 34.66 , 29.73 , 39.76 2024-11-06T04:44:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.01 2024-11-06T04:44:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868259.44 epoch sec; seqNo:1342; 4061676.02 north; 594375.81 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:44:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868256.98 2024-11-06T04:44:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:19Z- waiting for 1730868260.98, time is 1730868259.44 2024-11-06T04:44:19Z- AHRS_M2 msg: 1730868259.80 epoch sec; seqNo:1242 2024-11-06T04:44:19Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:44:19Z- handleDepth()- Depth_Keller msg: 1730868259.81 epoch sec; seqNo:1343; depth 49.91 2024-11-06T04:44:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868259.83 epoch sec; seqNo:1343; 4061676.04 north; 594377.03 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:44:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868256.98 2024-11-06T04:44:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:19Z- waiting for 1730868260.98, time is 1730868259.83 2024-11-06T04:44:20Z- AHRS_M2 msg: 1730868260.20 epoch sec; seqNo:1243 2024-11-06T04:44:20Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:44:20Z- handleDepth()- Depth_Keller msg: 1730868260.22 epoch sec; seqNo:1344; depth 49.92 2024-11-06T04:44:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868260.25 epoch sec; seqNo:1344; 4061676.04 north; 594377.03 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:44:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868256.98 2024-11-06T04:44:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:20Z- waiting for 1730868260.98, time is 1730868260.25 2024-11-06T04:44:20Z- AHRS_M2 msg: 1730868260.61 epoch sec; seqNo:1244 2024-11-06T04:44:20Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.49 psi 2024-11-06T04:44:20Z- handleDepth()- Depth_Keller msg: 1730868260.62 epoch sec; seqNo:1345; depth 49.94 2024-11-06T04:44:20Z- handleDVL() - DVL msg epochMillisec: 1730868260.65 2024-11-06T04:44:20Z- handleDvl() - RDI_Pathfinder msg: 1730868260.65 sec; seqNo:449; #beams:4 2024-11-06T04:44:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.36 , 34.66 , 29.73 , 40.10 2024-11-06T04:44:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.03 2024-11-06T04:44:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868260.66 epoch sec; seqNo:1345; 4061676.04 north; 594377.03 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T04:44:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868256.98 2024-11-06T04:44:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:20Z- waiting for 1730868260.98, time is 1730868260.66 2024-11-06T04:44:21Z- AHRS_M2 msg: 1730868261.01 epoch sec; seqNo:1245 2024-11-06T04:44:21Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.49 psi 2024-11-06T04:44:21Z- handleDepth()- Depth_Keller msg: 1730868261.02 epoch sec; seqNo:1346; depth 49.96 2024-11-06T04:44:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868261.05 epoch sec; seqNo:1346; 4061676.04 north; 594377.03 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:44:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868256.98 2024-11-06T04:44:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:21Z- waiting for 1730868260.98, time is 1730868261.05 2024-11-06T04:44:21Z- LcmTrn::updateTrn() >>>> pose time 1730868261.05, meas time 1730868260.65 <<<< 2024-11-06T04:44:21Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868261.05 2024-11-06T04:44:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868260.65 sec, ping # 0; waiting for INS data more recent than 1730868256.98... 2024-11-06T04:44:21Z- TerrainNav::motionUpdate() - currEstimate = (4061676, 594377.03, 49.96) 2024-11-06T04:44:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:44:21Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:44:21Z- TNavPF::Projecting Measurements 2024-11-06T04:44:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:44:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:44:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:44:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:44:21Z- TNavFilter::4 good beams. 2024-11-06T04:44:21Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:44:21Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:44:21Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:44:21Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:44:21Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:44:21Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:44:21Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:44:21Z- TerrainMapOctree: (vehN, vehE ) = (4061676.00, 594376.78). 2024-11-06T04:44:21Z- TerrainMapOctree: Min Distance = 146.80. 2024-11-06T04:44:21Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:44:21Z- Forcing Subcloud Comparison 2024-11-06T04:44:21Z- Weighting particles with subcloud comparison 2024-11-06T04:44:21Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:21Z- meanExpectedMeasDiff: -0.733995 delta_rms_squared: 1.938662 alpha: 0.989789 2024-11-06T04:44:21Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:21Z- meanExpectedMeasDiff: 0.360423 delta_rms_squared: 1.941546 alpha: 0.989804 2024-11-06T04:44:21Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:21Z- meanExpectedMeasDiff: -3.555566 delta_rms_squared: 1.559349 alpha: 0.987337 2024-11-06T04:44:21Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:21Z- meanExpectedMeasDiff: 1.851141 delta_rms_squared: 1.804491 alpha: 0.989038 2024-11-06T04:44:21Z- Subcloud NIS: 0.415637 2024-11-06T04:44:21Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:44:21Z- TNavPF:: sumWeights = 0.058329 2024-11-06T04:44:21Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:44:21Z- TNavPF::First Term of Map Covariance Matrix is 0.04 2024-11-06T04:44:21Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.01 2024-11-06T04:44:21Z- TerrainNav::Resampling particle filter... 2024-11-06T04:44:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868260.65 sec. 2024-11-06T04:44:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:44:21Z- variances([0], [2], and psi_berg) = 0.219099 0.286824 0.000000 2024-11-06T04:44:21Z- publishEstimatesDecoder() - at 1730868261.05 sec 2024-11-06T04:44:21Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:44:21Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061620.27 594381.74 54.28 2024-11-06T04:44:21Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061619.69 594381.14 53.97 lat/lon: 36.70 -121.94 2024-11-06T04:44:21Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.29 0.68 0.00 2024-11-06T04:44:21Z- AHRS_M2 msg: 1730868261.43 epoch sec; seqNo:1246 2024-11-06T04:44:21Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.49 psi 2024-11-06T04:44:21Z- handleDepth()- Depth_Keller msg: 1730868261.44 epoch sec; seqNo:1347; depth 49.98 2024-11-06T04:44:21Z- handleDVL() - DVL msg epochMillisec: 1730868261.46 2024-11-06T04:44:21Z- handleDvl() - RDI_Pathfinder msg: 1730868261.46 sec; seqNo:450; #beams:4 2024-11-06T04:44:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.36 , 34.49 , 29.73 , 39.42 2024-11-06T04:44:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.03 2024-11-06T04:44:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868261.48 epoch sec; seqNo:1347; 4061676.05 north; 594378.25 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T04:44:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868261.05 2024-11-06T04:44:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:21Z- waiting for 1730868265.05, time is 1730868261.48 2024-11-06T04:44:21Z- AHRS_M2 msg: 1730868261.83 epoch sec; seqNo:1247 2024-11-06T04:44:21Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.49 psi 2024-11-06T04:44:21Z- handleDepth()- Depth_Keller msg: 1730868261.83 epoch sec; seqNo:1348; depth 50.00 2024-11-06T04:44:21Z- handleDVL() - DVL msg epochMillisec: 1730868261.84 2024-11-06T04:44:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:44:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:44:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:44:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:44:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:44:21Z- handleDvl() - RDI_Pathfinder msg: 1730868261.84 sec; seqNo:451; #beams:0 2024-11-06T04:44:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:44:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.03 2024-11-06T04:44:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868261.86 epoch sec; seqNo:1348; 4061676.43 north; 594378.24 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T04:44:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868261.05 2024-11-06T04:44:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:21Z- waiting for 1730868265.05, time is 1730868261.86 2024-11-06T04:44:22Z- AHRS_M2 msg: 1730868262.23 epoch sec; seqNo:1248 2024-11-06T04:44:22Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.49 psi 2024-11-06T04:44:22Z- handleDepth()- Depth_Keller msg: 1730868262.23 epoch sec; seqNo:1349; depth 50.01 2024-11-06T04:44:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868262.24 epoch sec; seqNo:1349; 4061676.43 north; 594378.24 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:44:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868261.05 2024-11-06T04:44:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:22Z- waiting for 1730868265.05, time is 1730868262.24 2024-11-06T04:44:22Z- AHRS_M2 msg: 1730868262.63 epoch sec; seqNo:1249 2024-11-06T04:44:22Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:44:22Z- handleDepth()- Depth_Keller msg: 1730868262.63 epoch sec; seqNo:1350; depth 50.03 2024-11-06T04:44:22Z- handleDVL() - DVL msg epochMillisec: 1730868262.65 2024-11-06T04:44:22Z- handleDvl() - RDI_Pathfinder msg: 1730868262.65 sec; seqNo:452; #beams:4 2024-11-06T04:44:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.85 , 34.49 , 30.07 , 39.42 2024-11-06T04:44:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.03 2024-11-06T04:44:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868262.67 epoch sec; seqNo:1350; 4061676.44 north; 594379.46 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T04:44:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868261.05 2024-11-06T04:44:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:22Z- waiting for 1730868265.05, time is 1730868262.67 2024-11-06T04:44:23Z- AHRS_M2 msg: 1730868263.03 epoch sec; seqNo:1250 2024-11-06T04:44:23Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.49 psi 2024-11-06T04:44:23Z- handleDepth()- Depth_Keller msg: 1730868263.04 epoch sec; seqNo:1351; depth 50.05 2024-11-06T04:44:23Z- handleDVL() - DVL msg epochMillisec: 1730868263.05 2024-11-06T04:44:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:44:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:44:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:44:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:44:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:44:23Z- handleDvl() - RDI_Pathfinder msg: 1730868263.05 sec; seqNo:453; #beams:0 2024-11-06T04:44:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:44:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.03 2024-11-06T04:44:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868263.07 epoch sec; seqNo:1351; 4061676.44 north; 594379.46 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T04:44:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868261.05 2024-11-06T04:44:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:23Z- waiting for 1730868265.05, time is 1730868263.07 2024-11-06T04:44:23Z- AHRS_M2 msg: 1730868263.43 epoch sec; seqNo:1251 2024-11-06T04:44:23Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:44:23Z- handleDepth()- Depth_Keller msg: 1730868263.44 epoch sec; seqNo:1352; depth 50.05 2024-11-06T04:44:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868263.46 epoch sec; seqNo:1352; 4061676.44 north; 594379.46 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T04:44:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868261.05 2024-11-06T04:44:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:23Z- waiting for 1730868265.05, time is 1730868263.46 2024-11-06T04:44:23Z- AHRS_M2 msg: 1730868263.84 epoch sec; seqNo:1252 2024-11-06T04:44:23Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.50 psi 2024-11-06T04:44:23Z- handleDepth()- Depth_Keller msg: 1730868263.85 epoch sec; seqNo:1353; depth 50.08 2024-11-06T04:44:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868263.86 epoch sec; seqNo:1353; 4061676.44 north; 594379.46 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T04:44:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868261.05 2024-11-06T04:44:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:24Z- waiting for 1730868265.05, time is 1730868263.86 2024-11-06T04:44:24Z- AHRS_M2 msg: 1730868264.24 epoch sec; seqNo:1253 2024-11-06T04:44:24Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.50 psi 2024-11-06T04:44:24Z- handleDepth()- Depth_Keller msg: 1730868264.25 epoch sec; seqNo:1354; depth 50.08 2024-11-06T04:44:24Z- handleDVL() - DVL msg epochMillisec: 1730868264.25 2024-11-06T04:44:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:44:24Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:44:24Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:44:24Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:44:24Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:44:24Z- handleDvl() - RDI_Pathfinder msg: 1730868264.25 sec; seqNo:454; #beams:0 2024-11-06T04:44:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:44:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.03 2024-11-06T04:44:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868264.26 epoch sec; seqNo:1354; 4061676.46 north; 594380.68 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T04:44:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868261.05 2024-11-06T04:44:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:24Z- waiting for 1730868265.05, time is 1730868264.26 2024-11-06T04:44:24Z- AHRS_M2 msg: 1730868264.65 epoch sec; seqNo:1254 2024-11-06T04:44:24Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.50 psi 2024-11-06T04:44:24Z- handleDepth()- Depth_Keller msg: 1730868264.65 epoch sec; seqNo:1355; depth 50.10 2024-11-06T04:44:24Z- handleDVL() - DVL msg epochMillisec: 1730868264.68 2024-11-06T04:44:24Z- handleDvl() - RDI_Pathfinder msg: 1730868264.68 sec; seqNo:455; #beams:4 2024-11-06T04:44:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.32 , 33.81 , 30.07 , 38.91 2024-11-06T04:44:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , -0.01 2024-11-06T04:44:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868264.69 epoch sec; seqNo:1355; 4061676.46 north; 594380.68 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T04:44:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868261.05 2024-11-06T04:44:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:24Z- waiting for 1730868265.05, time is 1730868264.69 2024-11-06T04:44:25Z- AHRS_M2 msg: 1730868265.05 epoch sec; seqNo:1255 2024-11-06T04:44:25Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.50 psi 2024-11-06T04:44:25Z- handleDepth()- Depth_Keller msg: 1730868265.06 epoch sec; seqNo:1356; depth 50.08 2024-11-06T04:44:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868265.07 epoch sec; seqNo:1356; 4061676.46 north; 594380.68 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T04:44:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868261.05 2024-11-06T04:44:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:25Z- waiting for 1730868265.05, time is 1730868265.07 2024-11-06T04:44:25Z- LcmTrn::updateTrn() >>>> pose time 1730868265.07, meas time 1730868264.68 <<<< 2024-11-06T04:44:25Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868265.07 2024-11-06T04:44:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868264.68 sec, ping # 0; waiting for INS data more recent than 1730868261.05... 2024-11-06T04:44:25Z- TerrainNav::motionUpdate() - currEstimate = (4061676, 594380.68, 50.08) 2024-11-06T04:44:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:44:25Z- TerrainNav:: SubcloudNIS is 0.42, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:44:25Z- TNavPF::Projecting Measurements 2024-11-06T04:44:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:44:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:44:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:44:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:44:25Z- TNavFilter::4 good beams. 2024-11-06T04:44:25Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:44:25Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:44:25Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:44:25Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:44:25Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:44:25Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:44:25Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:44:25Z- TerrainMapOctree: (vehN, vehE ) = (4061676.42, 594380.33). 2024-11-06T04:44:25Z- TerrainMapOctree: Min Distance = 149.67. 2024-11-06T04:44:25Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:44:25Z- Forcing Subcloud Comparison 2024-11-06T04:44:25Z- Weighting particles with subcloud comparison 2024-11-06T04:44:25Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:25Z- meanExpectedMeasDiff: -2.005188 delta_rms_squared: 1.495021 alpha: 0.986799 2024-11-06T04:44:25Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:25Z- meanExpectedMeasDiff: 1.150911 delta_rms_squared: 1.663378 alpha: 0.988119 2024-11-06T04:44:25Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:25Z- meanExpectedMeasDiff: -2.689775 delta_rms_squared: 1.214234 alpha: 0.983796 2024-11-06T04:44:25Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:25Z- meanExpectedMeasDiff: 2.520170 delta_rms_squared: 1.333623 alpha: 0.985225 2024-11-06T04:44:25Z- Subcloud NIS: 0.447669 2024-11-06T04:44:25Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:44:25Z- TNavPF:: sumWeights = 0.049045 2024-11-06T04:44:25Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:44:25Z- TNavPF::First Term of Map Covariance Matrix is 0.16 2024-11-06T04:44:25Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T04:44:25Z- TerrainNav::Resampling particle filter... 2024-11-06T04:44:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868264.68 sec. 2024-11-06T04:44:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:44:25Z- variances([0], [2], and psi_berg) = 0.216843 0.278062 0.000000 2024-11-06T04:44:25Z- publishEstimatesDecoder() - at 1730868265.07 sec 2024-11-06T04:44:25Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:44:25Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061620.65 594385.37 54.35 2024-11-06T04:44:25Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061620.05 594384.73 54.29 lat/lon: 36.70 -121.94 2024-11-06T04:44:25Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.22 0.28 0.61 0.00 2024-11-06T04:44:25Z- AHRS_M2 msg: 1730868265.45 epoch sec; seqNo:1256 2024-11-06T04:44:25Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.50 psi 2024-11-06T04:44:25Z- handleDepth()- Depth_Keller msg: 1730868265.46 epoch sec; seqNo:1357; depth 50.12 2024-11-06T04:44:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868265.51 epoch sec; seqNo:1357; 4061676.47 north; 594381.90 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T04:44:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868265.07 2024-11-06T04:44:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:25Z- waiting for 1730868269.07, time is 1730868265.51 2024-11-06T04:44:25Z- AHRS_M2 msg: 1730868265.86 epoch sec; seqNo:1257 2024-11-06T04:44:25Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.51 psi 2024-11-06T04:44:25Z- handleDepth()- Depth_Keller msg: 1730868265.86 epoch sec; seqNo:1358; depth 50.12 2024-11-06T04:44:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868265.87 epoch sec; seqNo:1358; 4061676.47 north; 594381.90 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T04:44:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868265.07 2024-11-06T04:44:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:26Z- waiting for 1730868269.07, time is 1730868265.87 2024-11-06T04:44:26Z- AHRS_M2 msg: 1730868266.29 epoch sec; seqNo:1258 2024-11-06T04:44:26Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.51 psi 2024-11-06T04:44:26Z- handleDepth()- Depth_Keller msg: 1730868266.31 epoch sec; seqNo:1359; depth 50.13 2024-11-06T04:44:26Z- handleDVL() - DVL msg epochMillisec: 1730868266.34 2024-11-06T04:44:26Z- handleDvl() - RDI_Pathfinder msg: 1730868266.34 sec; seqNo:456; #beams:4 2024-11-06T04:44:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.11 , 32.88 , 29.82 , 38.38 2024-11-06T04:44:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , -0.05 2024-11-06T04:44:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868266.36 epoch sec; seqNo:1359; 4061676.85 north; 594381.90 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T04:44:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868265.07 2024-11-06T04:44:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:26Z- waiting for 1730868269.07, time is 1730868266.36 2024-11-06T04:44:26Z- AHRS_M2 msg: 1730868266.67 epoch sec; seqNo:1259 2024-11-06T04:44:26Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.51 psi 2024-11-06T04:44:26Z- handleDepth()- Depth_Keller msg: 1730868266.67 epoch sec; seqNo:1360; depth 50.14 2024-11-06T04:44:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868266.69 epoch sec; seqNo:1360; 4061676.86 north; 594383.12 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:44:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868265.07 2024-11-06T04:44:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:26Z- waiting for 1730868269.07, time is 1730868266.69 2024-11-06T04:44:27Z- AHRS_M2 msg: 1730868267.07 epoch sec; seqNo:1260 2024-11-06T04:44:27Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.51 psi 2024-11-06T04:44:27Z- handleDepth()- Depth_Keller msg: 1730868267.08 epoch sec; seqNo:1361; depth 50.14 2024-11-06T04:44:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868267.09 epoch sec; seqNo:1361; 4061676.86 north; 594383.12 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:44:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868265.07 2024-11-06T04:44:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:27Z- waiting for 1730868269.07, time is 1730868267.09 2024-11-06T04:44:27Z- AHRS_M2 msg: 1730868267.48 epoch sec; seqNo:1261 2024-11-06T04:44:27Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.51 psi 2024-11-06T04:44:27Z- handleDepth()- Depth_Keller msg: 1730868267.48 epoch sec; seqNo:1362; depth 50.14 2024-11-06T04:44:27Z- handleDVL() - DVL msg epochMillisec: 1730868267.50 2024-11-06T04:44:27Z- handleDvl() - RDI_Pathfinder msg: 1730868267.50 sec; seqNo:457; #beams:4 2024-11-06T04:44:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.42 , 32.42 , 29.97 , 38.08 2024-11-06T04:44:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , -0.06 2024-11-06T04:44:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868267.54 epoch sec; seqNo:1362; 4061676.86 north; 594383.12 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:44:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868265.07 2024-11-06T04:44:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:27Z- waiting for 1730868269.07, time is 1730868267.54 2024-11-06T04:44:27Z- AHRS_M2 msg: 1730868267.88 epoch sec; seqNo:1262 2024-11-06T04:44:27Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.51 psi 2024-11-06T04:44:27Z- handleDepth()- Depth_Keller msg: 1730868267.89 epoch sec; seqNo:1363; depth 50.14 2024-11-06T04:44:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868267.90 epoch sec; seqNo:1363; 4061676.86 north; 594383.12 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:44:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868265.07 2024-11-06T04:44:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:28Z- waiting for 1730868269.07, time is 1730868267.90 2024-11-06T04:44:28Z- AHRS_M2 msg: 1730868268.28 epoch sec; seqNo:1263 2024-11-06T04:44:28Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.51 psi 2024-11-06T04:44:28Z- handleDepth()- Depth_Keller msg: 1730868268.31 epoch sec; seqNo:1364; depth 50.15 2024-11-06T04:44:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868268.36 epoch sec; seqNo:1364; 4061676.88 north; 594384.34 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T04:44:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868265.07 2024-11-06T04:44:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:28Z- waiting for 1730868269.07, time is 1730868268.36 2024-11-06T04:44:28Z- AHRS_M2 msg: 1730868268.69 epoch sec; seqNo:1264 2024-11-06T04:44:28Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.51 psi 2024-11-06T04:44:28Z- handleDepth()- Depth_Keller msg: 1730868268.69 epoch sec; seqNo:1365; depth 50.15 2024-11-06T04:44:28Z- handleDVL() - DVL msg epochMillisec: 1730868268.71 2024-11-06T04:44:28Z- handleDvl() - RDI_Pathfinder msg: 1730868268.71 sec; seqNo:458; #beams:4 2024-11-06T04:44:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.96 , 32.11 , 29.82 , 37.77 2024-11-06T04:44:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , -0.07 2024-11-06T04:44:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868268.75 epoch sec; seqNo:1365; 4061676.88 north; 594384.34 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T04:44:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868265.07 2024-11-06T04:44:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:28Z- waiting for 1730868269.07, time is 1730868268.75 2024-11-06T04:44:29Z- AHRS_M2 msg: 1730868269.10 epoch sec; seqNo:1265 2024-11-06T04:44:29Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.51 psi 2024-11-06T04:44:29Z- handleDepth()- Depth_Keller msg: 1730868269.10 epoch sec; seqNo:1366; depth 50.17 2024-11-06T04:44:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868269.11 epoch sec; seqNo:1366; 4061676.88 north; 594384.34 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T04:44:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868265.07 2024-11-06T04:44:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:29Z- waiting for 1730868269.07, time is 1730868269.11 2024-11-06T04:44:29Z- LcmTrn::updateTrn() >>>> pose time 1730868269.11, meas time 1730868268.71 <<<< 2024-11-06T04:44:29Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868269.11 2024-11-06T04:44:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868268.71 sec, ping # 0; waiting for INS data more recent than 1730868265.07... 2024-11-06T04:44:29Z- TerrainNav::motionUpdate() - currEstimate = (4061677, 594384.34, 50.17) 2024-11-06T04:44:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:44:29Z- TerrainNav:: SubcloudNIS is 0.45, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:44:29Z- TNavPF::Projecting Measurements 2024-11-06T04:44:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:44:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:44:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:44:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:44:29Z- TNavFilter::4 good beams. 2024-11-06T04:44:29Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:44:29Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:44:29Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:44:29Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:44:29Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:44:29Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:44:29Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:44:29Z- TerrainMapOctree: (vehN, vehE ) = (4061676.83, 594383.98). 2024-11-06T04:44:29Z- TerrainMapOctree: Min Distance = 152.65. 2024-11-06T04:44:29Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:44:29Z- Forcing Subcloud Comparison 2024-11-06T04:44:29Z- Weighting particles with subcloud comparison 2024-11-06T04:44:29Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:29Z- meanExpectedMeasDiff: -3.717711 delta_rms_squared: 1.338969 alpha: 0.985283 2024-11-06T04:44:29Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:29Z- meanExpectedMeasDiff: 0.426459 delta_rms_squared: 1.196133 alpha: 0.983554 2024-11-06T04:44:29Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:29Z- meanExpectedMeasDiff: -2.692686 delta_rms_squared: 1.110172 alpha: 0.982304 2024-11-06T04:44:29Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:29Z- meanExpectedMeasDiff: 2.707714 delta_rms_squared: 1.848196 alpha: 0.989294 2024-11-06T04:44:29Z- Subcloud NIS: 0.481740 2024-11-06T04:44:29Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:44:29Z- TNavPF:: sumWeights = 0.014767 2024-11-06T04:44:29Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:44:29Z- TNavPF::First Term of Map Covariance Matrix is 0.15 2024-11-06T04:44:29Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:44:29Z- TerrainNav::Resampling particle filter... 2024-11-06T04:44:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868268.71 sec. 2024-11-06T04:44:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:44:29Z- variances([0], [2], and psi_berg) = 0.208048 0.274058 0.000000 2024-11-06T04:44:29Z- publishEstimatesDecoder() - at 1730868269.11 sec 2024-11-06T04:44:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:44:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061621.08 594389.04 54.08 2024-11-06T04:44:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061620.35 594388.25 54.79 lat/lon: 36.70 -121.94 2024-11-06T04:44:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.21 0.27 0.58 0.00 2024-11-06T04:44:29Z- AHRS_M2 msg: 1730868269.49 epoch sec; seqNo:1266 2024-11-06T04:44:29Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.51 psi 2024-11-06T04:44:29Z- handleDepth()- Depth_Keller msg: 1730868269.51 epoch sec; seqNo:1367; depth 50.17 2024-11-06T04:44:29Z- handleDVL() - DVL msg epochMillisec: 1730868269.54 2024-11-06T04:44:29Z- handleDvl() - RDI_Pathfinder msg: 1730868269.54 sec; seqNo:459; #beams:4 2024-11-06T04:44:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.50 , 31.81 , 29.51 , 37.01 2024-11-06T04:44:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , -0.06 2024-11-06T04:44:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868269.57 epoch sec; seqNo:1367; 4061676.89 north; 594385.56 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T04:44:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868269.11 2024-11-06T04:44:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:29Z- waiting for 1730868273.11, time is 1730868269.57 2024-11-06T04:44:29Z- AHRS_M2 msg: 1730868269.90 epoch sec; seqNo:1267 2024-11-06T04:44:29Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.51 psi 2024-11-06T04:44:29Z- handleDepth()- Depth_Keller msg: 1730868269.91 epoch sec; seqNo:1368; depth 50.17 2024-11-06T04:44:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868269.93 epoch sec; seqNo:1368; 4061676.89 north; 594385.56 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T04:44:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868269.11 2024-11-06T04:44:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:30Z- waiting for 1730868273.11, time is 1730868269.93 2024-11-06T04:44:30Z- AHRS_M2 msg: 1730868270.31 epoch sec; seqNo:1268 2024-11-06T04:44:30Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.50 psi 2024-11-06T04:44:30Z- handleDepth()- Depth_Keller msg: 1730868270.31 epoch sec; seqNo:1369; depth 50.19 2024-11-06T04:44:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868270.36 epoch sec; seqNo:1369; 4061677.27 north; 594385.56 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:44:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868269.11 2024-11-06T04:44:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:30Z- waiting for 1730868273.11, time is 1730868270.36 2024-11-06T04:44:30Z- handleDepth()- Depth_Keller msg: 1730868270.73 epoch sec; seqNo:1370; depth 50.19 2024-11-06T04:44:30Z- handleDVL() - DVL msg epochMillisec: 1730868270.75 2024-11-06T04:44:30Z- handleDvl() - RDI_Pathfinder msg: 1730868270.75 sec; seqNo:460; #beams:4 2024-11-06T04:44:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.65 , 31.04 , 29.67 , 36.85 2024-11-06T04:44:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , -0.04 2024-11-06T04:44:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868270.77 epoch sec; seqNo:1370; 4061677.27 north; 594385.56 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:44:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868269.11 2024-11-06T04:44:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:30Z- waiting for 1730868273.11, time is 1730868270.77 2024-11-06T04:44:31Z- AHRS_M2 msg: 1730868271.11 epoch sec; seqNo:1269 2024-11-06T04:44:31Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.50 psi 2024-11-06T04:44:31Z- handleDepth()- Depth_Keller msg: 1730868271.13 epoch sec; seqNo:1371; depth 50.20 2024-11-06T04:44:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868271.14 epoch sec; seqNo:1371; 4061677.28 north; 594386.78 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:44:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868269.11 2024-11-06T04:44:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:31Z- waiting for 1730868273.11, time is 1730868271.14 2024-11-06T04:44:31Z- AHRS_M2 msg: 1730868271.52 epoch sec; seqNo:1270 2024-11-06T04:44:31Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.50 psi 2024-11-06T04:44:31Z- handleDepth()- Depth_Keller msg: 1730868271.53 epoch sec; seqNo:1372; depth 50.23 2024-11-06T04:44:31Z- handleDVL() - DVL msg epochMillisec: 1730868271.55 2024-11-06T04:44:31Z- handleDvl() - RDI_Pathfinder msg: 1730868271.55 sec; seqNo:461; #beams:4 2024-11-06T04:44:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.81 , 29.82 , 29.21 , 36.85 2024-11-06T04:44:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , -0.01 2024-11-06T04:44:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868271.57 epoch sec; seqNo:1372; 4061677.28 north; 594386.78 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T04:44:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868269.11 2024-11-06T04:44:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:31Z- waiting for 1730868273.11, time is 1730868271.57 2024-11-06T04:44:31Z- AHRS_M2 msg: 1730868271.92 epoch sec; seqNo:1271 2024-11-06T04:44:31Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.50 psi 2024-11-06T04:44:31Z- handleDepth()- Depth_Keller msg: 1730868271.93 epoch sec; seqNo:1373; depth 50.23 2024-11-06T04:44:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868271.94 epoch sec; seqNo:1373; 4061677.28 north; 594386.78 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T04:44:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868269.11 2024-11-06T04:44:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:32Z- waiting for 1730868273.11, time is 1730868271.94 2024-11-06T04:44:32Z- AHRS_M2 msg: 1730868272.33 epoch sec; seqNo:1272 2024-11-06T04:44:32Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.50 psi 2024-11-06T04:44:32Z- handleDepth()- Depth_Keller msg: 1730868272.33 epoch sec; seqNo:1374; depth 50.23 2024-11-06T04:44:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868272.35 epoch sec; seqNo:1374; 4061677.28 north; 594386.78 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T04:44:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868269.11 2024-11-06T04:44:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:32Z- waiting for 1730868273.11, time is 1730868272.35 2024-11-06T04:44:32Z- handleDepth()- Depth_Keller msg: 1730868272.75 epoch sec; seqNo:1375; depth 50.27 2024-11-06T04:44:32Z- handleDVL() - DVL msg epochMillisec: 1730868272.77 2024-11-06T04:44:32Z- handleDvl() - RDI_Pathfinder msg: 1730868272.77 sec; seqNo:462; #beams:4 2024-11-06T04:44:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.81 , 29.67 , 28.90 , 36.70 2024-11-06T04:44:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , 0.02 2024-11-06T04:44:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868272.78 epoch sec; seqNo:1375; 4061677.29 north; 594388.00 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T04:44:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868269.11 2024-11-06T04:44:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:32Z- waiting for 1730868273.11, time is 1730868272.78 2024-11-06T04:44:33Z- AHRS_M2 msg: 1730868273.13 epoch sec; seqNo:1273 2024-11-06T04:44:33Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.49 psi 2024-11-06T04:44:33Z- handleDepth()- Depth_Keller msg: 1730868273.14 epoch sec; seqNo:1376; depth 50.27 2024-11-06T04:44:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868273.17 epoch sec; seqNo:1376; 4061677.29 north; 594388.00 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T04:44:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868269.11 2024-11-06T04:44:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:33Z- waiting for 1730868273.11, time is 1730868273.17 2024-11-06T04:44:33Z- LcmTrn::updateTrn() >>>> pose time 1730868273.17, meas time 1730868272.77 <<<< 2024-11-06T04:44:33Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868273.17 2024-11-06T04:44:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868272.77 sec, ping # 0; waiting for INS data more recent than 1730868269.11... 2024-11-06T04:44:33Z- TerrainNav::motionUpdate() - currEstimate = (4061677, 594388.00, 50.27) 2024-11-06T04:44:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:44:33Z- TerrainNav:: SubcloudNIS is 0.48, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:44:33Z- TNavPF::Projecting Measurements 2024-11-06T04:44:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:44:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:44:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:44:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:44:33Z- TNavFilter::4 good beams. 2024-11-06T04:44:33Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:44:33Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:44:33Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:44:33Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:44:33Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:44:33Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:44:33Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:44:33Z- TerrainMapOctree: (vehN, vehE ) = (4061677.25, 594387.64). 2024-11-06T04:44:33Z- TerrainMapOctree: Min Distance = 155.68. 2024-11-06T04:44:33Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:44:33Z- Forcing Subcloud Comparison 2024-11-06T04:44:33Z- Weighting particles with subcloud comparison 2024-11-06T04:44:33Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:33Z- meanExpectedMeasDiff: -3.156738 delta_rms_squared: 1.326806 alpha: 0.985150 2024-11-06T04:44:33Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:33Z- meanExpectedMeasDiff: -1.180908 delta_rms_squared: 1.747324 alpha: 0.988683 2024-11-06T04:44:33Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:33Z- meanExpectedMeasDiff: -2.934585 delta_rms_squared: 0.996758 alpha: 0.980330 2024-11-06T04:44:33Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:33Z- meanExpectedMeasDiff: 3.814689 delta_rms_squared: 1.544019 alpha: 0.987212 2024-11-06T04:44:33Z- Subcloud NIS: 0.517087 2024-11-06T04:44:33Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:44:33Z- TNavPF:: sumWeights = 0.006112 2024-11-06T04:44:33Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:44:33Z- TNavPF::First Term of Map Covariance Matrix is 0.04 2024-11-06T04:44:33Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:44:33Z- TerrainNav::Resampling particle filter... 2024-11-06T04:44:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868272.77 sec. 2024-11-06T04:44:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:44:33Z- variances([0], [2], and psi_berg) = 0.243169 0.336322 0.000000 2024-11-06T04:44:33Z- publishEstimatesDecoder() - at 1730868273.17 sec 2024-11-06T04:44:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:44:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061621.40 594392.74 54.37 2024-11-06T04:44:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061620.80 594391.91 55.37 lat/lon: 36.70 -121.94 2024-11-06T04:44:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.34 0.59 0.00 2024-11-06T04:44:33Z- AHRS_M2 msg: 1730868273.54 epoch sec; seqNo:1274 2024-11-06T04:44:33Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.49 psi 2024-11-06T04:44:33Z- handleDepth()- Depth_Keller msg: 1730868273.54 epoch sec; seqNo:1377; depth 50.29 2024-11-06T04:44:33Z- handleDVL() - DVL msg epochMillisec: 1730868273.56 2024-11-06T04:44:33Z- handleDvl() - RDI_Pathfinder msg: 1730868273.56 sec; seqNo:463; #beams:4 2024-11-06T04:44:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.96 , 31.04 , 28.75 , 36.85 2024-11-06T04:44:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.05 2024-11-06T04:44:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868273.59 epoch sec; seqNo:1377; 4061677.29 north; 594388.00 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:44:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868273.17 2024-11-06T04:44:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:33Z- waiting for 1730868277.17, time is 1730868273.59 2024-11-06T04:44:33Z- AHRS_M2 msg: 1730868273.94 epoch sec; seqNo:1275 2024-11-06T04:44:33Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.49 psi 2024-11-06T04:44:33Z- handleDepth()- Depth_Keller msg: 1730868273.94 epoch sec; seqNo:1378; depth 50.30 2024-11-06T04:44:33Z- handleDVL() - DVL msg epochMillisec: 1730868273.95 2024-11-06T04:44:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:44:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:44:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:44:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:44:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:44:33Z- handleDvl() - RDI_Pathfinder msg: 1730868273.95 sec; seqNo:464; #beams:0 2024-11-06T04:44:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:44:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.05 2024-11-06T04:44:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868273.97 epoch sec; seqNo:1378; 4061677.69 north; 594389.21 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:44:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868273.17 2024-11-06T04:44:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:34Z- waiting for 1730868277.17, time is 1730868273.97 2024-11-06T04:44:34Z- AHRS_M2 msg: 1730868274.34 epoch sec; seqNo:1276 2024-11-06T04:44:34Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.49 psi 2024-11-06T04:44:34Z- handleDepth()- Depth_Keller msg: 1730868274.36 epoch sec; seqNo:1379; depth 50.31 2024-11-06T04:44:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868274.40 epoch sec; seqNo:1379; 4061677.69 north; 594389.21 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:44:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868273.17 2024-11-06T04:44:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:34Z- waiting for 1730868277.17, time is 1730868274.40 2024-11-06T04:44:34Z- AHRS_M2 msg: 1730868274.76 epoch sec; seqNo:1277 2024-11-06T04:44:34Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.49 psi 2024-11-06T04:44:34Z- handleDepth()- Depth_Keller msg: 1730868274.76 epoch sec; seqNo:1380; depth 50.33 2024-11-06T04:44:34Z- handleDVL() - DVL msg epochMillisec: 1730868274.78 2024-11-06T04:44:34Z- handleDvl() - RDI_Pathfinder msg: 1730868274.78 sec; seqNo:465; #beams:4 2024-11-06T04:44:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.11 , 31.04 , 28.59 , 36.85 2024-11-06T04:44:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.06 2024-11-06T04:44:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868274.79 epoch sec; seqNo:1380; 4061677.69 north; 594389.21 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T04:44:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868273.17 2024-11-06T04:44:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:34Z- waiting for 1730868277.17, time is 1730868274.79 2024-11-06T04:44:35Z- AHRS_M2 msg: 1730868275.15 epoch sec; seqNo:1278 2024-11-06T04:44:35Z- AHRS_M2 msg: 0.12 phi; -0.02 theta; 1.49 psi 2024-11-06T04:44:35Z- handleDepth()- Depth_Keller msg: 1730868275.16 epoch sec; seqNo:1381; depth 50.34 2024-11-06T04:44:35Z- handleDVL() - DVL msg epochMillisec: 1730868275.16 2024-11-06T04:44:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:44:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:44:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:44:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:44:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:44:35Z- handleDvl() - RDI_Pathfinder msg: 1730868275.16 sec; seqNo:466; #beams:0 2024-11-06T04:44:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:44:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.06 2024-11-06T04:44:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868275.18 epoch sec; seqNo:1381; 4061677.69 north; 594389.21 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:44:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868273.17 2024-11-06T04:44:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:35Z- waiting for 1730868277.17, time is 1730868275.18 2024-11-06T04:44:35Z- AHRS_M2 msg: 1730868275.56 epoch sec; seqNo:1279 2024-11-06T04:44:35Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.49 psi 2024-11-06T04:44:35Z- handleDepth()- Depth_Keller msg: 1730868275.57 epoch sec; seqNo:1382; depth 50.37 2024-11-06T04:44:35Z- handleDVL() - DVL msg epochMillisec: 1730868275.59 2024-11-06T04:44:35Z- handleDvl() - RDI_Pathfinder msg: 1730868275.59 sec; seqNo:467; #beams:4 2024-11-06T04:44:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.11 , 28.75 , 28.90 , 37.01 2024-11-06T04:44:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , 0.06 2024-11-06T04:44:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868275.61 epoch sec; seqNo:1382; 4061677.70 north; 594390.43 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:44:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868273.17 2024-11-06T04:44:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:35Z- waiting for 1730868277.17, time is 1730868275.61 2024-11-06T04:44:35Z- AHRS_M2 msg: 1730868275.96 epoch sec; seqNo:1280 2024-11-06T04:44:35Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.49 psi 2024-11-06T04:44:35Z- handleDepth()- Depth_Keller msg: 1730868275.97 epoch sec; seqNo:1383; depth 50.37 2024-11-06T04:44:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868275.99 epoch sec; seqNo:1383; 4061677.70 north; 594390.43 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:44:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868273.17 2024-11-06T04:44:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:36Z- waiting for 1730868277.17, time is 1730868275.99 2024-11-06T04:44:36Z- AHRS_M2 msg: 1730868276.36 epoch sec; seqNo:1281 2024-11-06T04:44:36Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.49 psi 2024-11-06T04:44:36Z- handleDepth()- Depth_Keller msg: 1730868276.38 epoch sec; seqNo:1384; depth 50.37 2024-11-06T04:44:36Z- handleDVL() - DVL msg epochMillisec: 1730868276.38 2024-11-06T04:44:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:44:36Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:44:36Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:44:36Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:44:36Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:44:36Z- handleDvl() - RDI_Pathfinder msg: 1730868276.38 sec; seqNo:468; #beams:0 2024-11-06T04:44:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:44:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , 0.06 2024-11-06T04:44:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868276.39 epoch sec; seqNo:1384; 4061677.70 north; 594390.43 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:44:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868273.17 2024-11-06T04:44:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:36Z- waiting for 1730868277.17, time is 1730868276.39 2024-11-06T04:44:36Z- AHRS_M2 msg: 1730868276.79 epoch sec; seqNo:1282 2024-11-06T04:44:36Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.49 psi 2024-11-06T04:44:36Z- handleDepth()- Depth_Keller msg: 1730868276.80 epoch sec; seqNo:1385; depth 50.38 2024-11-06T04:44:36Z- handleDVL() - DVL msg epochMillisec: 1730868276.83 2024-11-06T04:44:36Z- handleDvl() - RDI_Pathfinder msg: 1730868276.83 sec; seqNo:469; #beams:4 2024-11-06T04:44:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.96 , 27.83 , 28.59 , 37.01 2024-11-06T04:44:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , 0.03 2024-11-06T04:44:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868276.85 epoch sec; seqNo:1385; 4061677.71 north; 594391.65 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:44:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868273.17 2024-11-06T04:44:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:36Z- waiting for 1730868277.17, time is 1730868276.85 2024-11-06T04:44:37Z- AHRS_M2 msg: 1730868277.17 epoch sec; seqNo:1283 2024-11-06T04:44:37Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.49 psi 2024-11-06T04:44:37Z- handleDepth()- Depth_Keller msg: 1730868277.18 epoch sec; seqNo:1386; depth 50.39 2024-11-06T04:44:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868277.19 epoch sec; seqNo:1386; 4061677.71 north; 594391.65 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:44:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868273.17 2024-11-06T04:44:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:37Z- waiting for 1730868277.17, time is 1730868277.19 2024-11-06T04:44:37Z- LcmTrn::updateTrn() >>>> pose time 1730868277.19, meas time 1730868276.83 <<<< 2024-11-06T04:44:37Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868277.19 2024-11-06T04:44:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868276.83 sec, ping # 0; waiting for INS data more recent than 1730868273.17... 2024-11-06T04:44:37Z- TerrainNav::motionUpdate() - currEstimate = (4061678, 594391.65, 50.39) 2024-11-06T04:44:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:44:37Z- TerrainNav:: SubcloudNIS is 0.52, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:44:37Z- TNavPF::Projecting Measurements 2024-11-06T04:44:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:44:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:44:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:44:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:44:37Z- TNavFilter::4 good beams. 2024-11-06T04:44:37Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:44:37Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:44:37Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:44:37Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:44:37Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:44:37Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:44:37Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:44:37Z- TerrainMapOctree: (vehN, vehE ) = (4061677.68, 594391.32). 2024-11-06T04:44:37Z- TerrainMapOctree: Min Distance = 158.75. 2024-11-06T04:44:37Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:44:37Z- Forcing Subcloud Comparison 2024-11-06T04:44:37Z- Weighting particles with subcloud comparison 2024-11-06T04:44:37Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:37Z- meanExpectedMeasDiff: -2.309747 delta_rms_squared: 1.309033 alpha: 0.984951 2024-11-06T04:44:37Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:37Z- meanExpectedMeasDiff: -1.621533 delta_rms_squared: 0.961723 alpha: 0.979628 2024-11-06T04:44:37Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:37Z- meanExpectedMeasDiff: -1.657192 delta_rms_squared: 1.078126 alpha: 0.981787 2024-11-06T04:44:37Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:37Z- meanExpectedMeasDiff: 5.032061 delta_rms_squared: 1.135150 alpha: 0.982686 2024-11-06T04:44:37Z- Subcloud NIS: 0.555407 2024-11-06T04:44:37Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:44:37Z- TNavPF:: sumWeights = 0.005506 2024-11-06T04:44:37Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:44:37Z- TNavPF::First Term of Map Covariance Matrix is 0.03 2024-11-06T04:44:37Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:44:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868276.83 sec. 2024-11-06T04:44:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:44:37Z- variances([0], [2], and psi_berg) = 0.241637 0.340379 0.000000 2024-11-06T04:44:37Z- publishEstimatesDecoder() - at 1730868277.19 sec 2024-11-06T04:44:37Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:44:37Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061621.30 594397.32 54.98 2024-11-06T04:44:37Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061621.21 594395.56 55.67 lat/lon: 36.70 -121.94 2024-11-06T04:44:37Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.34 0.61 0.00 2024-11-06T04:44:37Z- AHRS_M2 msg: 1730868277.57 epoch sec; seqNo:1284 2024-11-06T04:44:37Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.49 psi 2024-11-06T04:44:37Z- handleDepth()- Depth_Keller msg: 1730868277.59 epoch sec; seqNo:1387; depth 50.38 2024-11-06T04:44:37Z- handleDVL() - DVL msg epochMillisec: 1730868277.62 2024-11-06T04:44:37Z- handleDvl() - RDI_Pathfinder msg: 1730868277.62 sec; seqNo:470; #beams:4 2024-11-06T04:44:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.81 , 28.29 , 21.41 , 37.01 2024-11-06T04:44:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.01 , 0.04 2024-11-06T04:44:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868277.64 epoch sec; seqNo:1387; 4061678.09 north; 594391.65 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:44:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868277.19 2024-11-06T04:44:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:37Z- waiting for 1730868281.19, time is 1730868277.64 2024-11-06T04:44:37Z- AHRS_M2 msg: 1730868277.98 epoch sec; seqNo:1285 2024-11-06T04:44:37Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.49 psi 2024-11-06T04:44:37Z- handleDepth()- Depth_Keller msg: 1730868277.99 epoch sec; seqNo:1388; depth 50.38 2024-11-06T04:44:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868278.01 epoch sec; seqNo:1388; 4061678.09 north; 594391.65 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:44:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868277.19 2024-11-06T04:44:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:38Z- waiting for 1730868281.19, time is 1730868278.01 2024-11-06T04:44:38Z- handleDepth()- Depth_Keller msg: 1730868278.40 epoch sec; seqNo:1389; depth 50.39 2024-11-06T04:44:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868278.41 epoch sec; seqNo:1389; 4061678.11 north; 594392.87 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:44:38Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:44:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868277.19 2024-11-06T04:44:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:38Z- waiting for 1730868281.19, time is 1730868278.41 2024-11-06T04:44:38Z- AHRS_M2 msg: 1730868278.79 epoch sec; seqNo:1286 2024-11-06T04:44:38Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.49 psi 2024-11-06T04:44:38Z- handleDepth()- Depth_Keller msg: 1730868278.80 epoch sec; seqNo:1390; depth 50.38 2024-11-06T04:44:38Z- handleDVL() - DVL msg epochMillisec: 1730868278.84 2024-11-06T04:44:38Z- handleDvl() - RDI_Pathfinder msg: 1730868278.84 sec; seqNo:471; #beams:4 2024-11-06T04:44:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.81 , 28.14 , 30.04 , 36.29 2024-11-06T04:44:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , -0.03 2024-11-06T04:44:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868278.85 epoch sec; seqNo:1390; 4061678.11 north; 594392.87 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:44:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868277.19 2024-11-06T04:44:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:38Z- waiting for 1730868281.19, time is 1730868278.85 2024-11-06T04:44:39Z- AHRS_M2 msg: 1730868279.21 epoch sec; seqNo:1287 2024-11-06T04:44:39Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.50 psi 2024-11-06T04:44:39Z- handleDepth()- Depth_Keller msg: 1730868279.21 epoch sec; seqNo:1391; depth 50.38 2024-11-06T04:44:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868279.22 epoch sec; seqNo:1391; 4061678.11 north; 594392.87 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:44:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868277.19 2024-11-06T04:44:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:39Z- waiting for 1730868281.19, time is 1730868279.22 2024-11-06T04:44:39Z- AHRS_M2 msg: 1730868279.60 epoch sec; seqNo:1288 2024-11-06T04:44:39Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T04:44:39Z- handleDepth()- Depth_Keller msg: 1730868279.62 epoch sec; seqNo:1392; depth 50.37 2024-11-06T04:44:39Z- handleDVL() - DVL msg epochMillisec: 1730868279.63 2024-11-06T04:44:39Z- handleDvl() - RDI_Pathfinder msg: 1730868279.63 sec; seqNo:472; #beams:4 2024-11-06T04:44:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.50 , 28.14 , 29.82 , 35.78 2024-11-06T04:44:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , -0.05 2024-11-06T04:44:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868279.65 epoch sec; seqNo:1392; 4061678.12 north; 594394.09 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:44:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868277.19 2024-11-06T04:44:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:39Z- waiting for 1730868281.19, time is 1730868279.65 2024-11-06T04:44:40Z- AHRS_M2 msg: 1730868280.01 epoch sec; seqNo:1289 2024-11-06T04:44:40Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T04:44:40Z- handleDepth()- Depth_Keller msg: 1730868280.01 epoch sec; seqNo:1393; depth 50.36 2024-11-06T04:44:40Z- handleDVL() - DVL msg epochMillisec: 1730868280.02 2024-11-06T04:44:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:44:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:44:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:44:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:44:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:44:40Z- handleDvl() - RDI_Pathfinder msg: 1730868280.02 sec; seqNo:473; #beams:0 2024-11-06T04:44:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:44:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , -0.05 2024-11-06T04:44:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868280.04 epoch sec; seqNo:1393; 4061678.12 north; 594394.09 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:44:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868277.19 2024-11-06T04:44:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:40Z- waiting for 1730868281.19, time is 1730868280.04 2024-11-06T04:44:40Z- AHRS_M2 msg: 1730868280.40 epoch sec; seqNo:1290 2024-11-06T04:44:40Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.50 psi 2024-11-06T04:44:40Z- handleDepth()- Depth_Keller msg: 1730868280.41 epoch sec; seqNo:1394; depth 50.36 2024-11-06T04:44:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868280.42 epoch sec; seqNo:1394; 4061678.12 north; 594394.09 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:44:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868277.19 2024-11-06T04:44:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:40Z- waiting for 1730868281.19, time is 1730868280.42 2024-11-06T04:44:40Z- AHRS_M2 msg: 1730868280.81 epoch sec; seqNo:1291 2024-11-06T04:44:40Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.50 psi 2024-11-06T04:44:40Z- handleDepth()- Depth_Keller msg: 1730868280.83 epoch sec; seqNo:1395; depth 50.35 2024-11-06T04:44:40Z- handleDVL() - DVL msg epochMillisec: 1730868280.86 2024-11-06T04:44:40Z- handleDvl() - RDI_Pathfinder msg: 1730868280.86 sec; seqNo:474; #beams:4 2024-11-06T04:44:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.81 , 27.68 , 30.28 , 34.87 2024-11-06T04:44:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , -0.08 2024-11-06T04:44:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868280.88 epoch sec; seqNo:1395; 4061678.13 north; 594395.31 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:44:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868277.19 2024-11-06T04:44:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:40Z- waiting for 1730868281.19, time is 1730868280.88 2024-11-06T04:44:41Z- AHRS_M2 msg: 1730868281.21 epoch sec; seqNo:1292 2024-11-06T04:44:41Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.49 psi 2024-11-06T04:44:41Z- handleDepth()- Depth_Keller msg: 1730868281.22 epoch sec; seqNo:1396; depth 50.36 2024-11-06T04:44:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868281.24 epoch sec; seqNo:1396; 4061678.13 north; 594395.31 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:44:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868277.19 2024-11-06T04:44:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:41Z- waiting for 1730868281.19, time is 1730868281.24 2024-11-06T04:44:41Z- LcmTrn::updateTrn() >>>> pose time 1730868281.24, meas time 1730868280.86 <<<< 2024-11-06T04:44:41Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868281.24 2024-11-06T04:44:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868280.86 sec, ping # 0; waiting for INS data more recent than 1730868277.19... 2024-11-06T04:44:41Z- TerrainNav::motionUpdate() - currEstimate = (4061678, 594395.31, 50.36) 2024-11-06T04:44:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:44:41Z- TerrainNav:: SubcloudNIS is 0.56, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:44:41Z- TNavPF::Projecting Measurements 2024-11-06T04:44:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:44:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:44:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:44:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:44:41Z- TNavFilter::4 good beams. 2024-11-06T04:44:41Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:44:41Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:44:41Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:44:41Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:44:41Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:44:41Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:44:41Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:44:41Z- TerrainMapOctree: (vehN, vehE ) = (4061678.09, 594394.96). 2024-11-06T04:44:41Z- TerrainMapOctree: Min Distance = 161.82. 2024-11-06T04:44:41Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:44:41Z- Forcing Subcloud Comparison 2024-11-06T04:44:41Z- Weighting particles with subcloud comparison 2024-11-06T04:44:41Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:41Z- meanExpectedMeasDiff: -1.063184 delta_rms_squared: 1.355858 alpha: 0.985464 2024-11-06T04:44:41Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:41Z- meanExpectedMeasDiff: -1.585584 delta_rms_squared: 0.999855 alpha: 0.980389 2024-11-06T04:44:41Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:41Z- meanExpectedMeasDiff: 0.177894 delta_rms_squared: 1.100326 alpha: 0.982148 2024-11-06T04:44:41Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:41Z- meanExpectedMeasDiff: 3.114273 delta_rms_squared: 1.178807 alpha: 0.983317 2024-11-06T04:44:41Z- Subcloud NIS: 0.573508 2024-11-06T04:44:41Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:44:41Z- TNavPF:: sumWeights = 0.097940 2024-11-06T04:44:41Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:44:41Z- TNavPF::First Term of Map Covariance Matrix is 0.15 2024-11-06T04:44:41Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T04:44:41Z- TerrainNav::Resampling particle filter... 2024-11-06T04:44:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868280.86 sec. 2024-11-06T04:44:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:44:41Z- variances([0], [2], and psi_berg) = 0.244135 0.343775 0.000000 2024-11-06T04:44:41Z- publishEstimatesDecoder() - at 1730868281.24 sec 2024-11-06T04:44:41Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:44:41Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061622.28 594400.08 55.61 2024-11-06T04:44:41Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061621.63 594399.22 55.61 lat/lon: 36.70 -121.94 2024-11-06T04:44:41Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.24 0.34 0.61 0.00 2024-11-06T04:44:41Z- handleDepth()- Depth_Keller msg: 1730868281.64 epoch sec; seqNo:1397; depth 50.34 2024-11-06T04:44:41Z- handleDVL() - DVL msg epochMillisec: 1730868281.68 2024-11-06T04:44:41Z- handleDvl() - RDI_Pathfinder msg: 1730868281.68 sec; seqNo:475; #beams:4 2024-11-06T04:44:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.50 , 27.98 , 30.43 , 33.95 2024-11-06T04:44:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , -0.08 2024-11-06T04:44:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868281.69 epoch sec; seqNo:1397; 4061678.51 north; 594395.31 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:44:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868281.24 2024-11-06T04:44:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:41Z- waiting for 1730868285.24, time is 1730868281.69 2024-11-06T04:44:42Z- AHRS_M2 msg: 1730868282.02 epoch sec; seqNo:1293 2024-11-06T04:44:42Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.49 psi 2024-11-06T04:44:42Z- handleDepth()- Depth_Keller msg: 1730868282.03 epoch sec; seqNo:1398; depth 50.33 2024-11-06T04:44:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868282.05 epoch sec; seqNo:1398; 4061678.51 north; 594395.31 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T04:44:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868281.24 2024-11-06T04:44:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:42Z- waiting for 1730868285.24, time is 1730868282.05 2024-11-06T04:44:42Z- AHRS_M2 msg: 1730868282.42 epoch sec; seqNo:1294 2024-11-06T04:44:42Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.49 psi 2024-11-06T04:44:42Z- handleDepth()- Depth_Keller msg: 1730868282.43 epoch sec; seqNo:1399; depth 50.34 2024-11-06T04:44:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868282.44 epoch sec; seqNo:1399; 4061678.53 north; 594396.53 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:44:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868281.24 2024-11-06T04:44:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:42Z- waiting for 1730868285.24, time is 1730868282.44 2024-11-06T04:44:42Z- AHRS_M2 msg: 1730868282.83 epoch sec; seqNo:1295 2024-11-06T04:44:42Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.49 psi 2024-11-06T04:44:42Z- handleDepth()- Depth_Keller msg: 1730868282.84 epoch sec; seqNo:1400; depth 50.34 2024-11-06T04:44:42Z- handleDVL() - DVL msg epochMillisec: 1730868282.86 2024-11-06T04:44:42Z- handleDvl() - RDI_Pathfinder msg: 1730868282.86 sec; seqNo:476; #beams:4 2024-11-06T04:44:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.65 , 28.14 , 30.43 , 33.95 2024-11-06T04:44:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.09 , -0.07 2024-11-06T04:44:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868282.89 epoch sec; seqNo:1400; 4061678.53 north; 594396.53 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:44:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868281.24 2024-11-06T04:44:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:42Z- waiting for 1730868285.24, time is 1730868282.89 2024-11-06T04:44:43Z- AHRS_M2 msg: 1730868283.23 epoch sec; seqNo:1296 2024-11-06T04:44:43Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.49 psi 2024-11-06T04:44:43Z- handleDepth()- Depth_Keller msg: 1730868283.24 epoch sec; seqNo:1401; depth 50.34 2024-11-06T04:44:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868283.25 epoch sec; seqNo:1401; 4061678.53 north; 594396.53 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:44:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868281.24 2024-11-06T04:44:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:43Z- waiting for 1730868285.24, time is 1730868283.25 2024-11-06T04:44:43Z- AHRS_M2 msg: 1730868283.63 epoch sec; seqNo:1297 2024-11-06T04:44:43Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.49 psi 2024-11-06T04:44:43Z- handleDepth()- Depth_Keller msg: 1730868283.64 epoch sec; seqNo:1402; depth 50.33 2024-11-06T04:44:43Z- handleDVL() - DVL msg epochMillisec: 1730868283.67 2024-11-06T04:44:43Z- handleDvl() - RDI_Pathfinder msg: 1730868283.67 sec; seqNo:477; #beams:4 2024-11-06T04:44:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.50 , 28.29 , 30.58 , 32.57 2024-11-06T04:44:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.08 , -0.05 2024-11-06T04:44:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868283.69 epoch sec; seqNo:1402; 4061678.53 north; 594396.53 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T04:44:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868281.24 2024-11-06T04:44:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:43Z- waiting for 1730868285.24, time is 1730868283.69 2024-11-06T04:44:44Z- AHRS_M2 msg: 1730868284.04 epoch sec; seqNo:1298 2024-11-06T04:44:44Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.48 psi 2024-11-06T04:44:44Z- handleDepth()- Depth_Keller msg: 1730868284.04 epoch sec; seqNo:1403; depth 50.34 2024-11-06T04:44:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868284.06 epoch sec; seqNo:1403; 4061678.54 north; 594397.75 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:44:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868281.24 2024-11-06T04:44:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:44Z- waiting for 1730868285.24, time is 1730868284.06 2024-11-06T04:44:44Z- AHRS_M2 msg: 1730868284.44 epoch sec; seqNo:1299 2024-11-06T04:44:44Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.48 psi 2024-11-06T04:44:44Z- handleDepth()- Depth_Keller msg: 1730868284.45 epoch sec; seqNo:1404; depth 50.34 2024-11-06T04:44:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868284.46 epoch sec; seqNo:1404; 4061678.92 north; 594397.74 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:44:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868281.24 2024-11-06T04:44:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:44Z- waiting for 1730868285.24, time is 1730868284.46 2024-11-06T04:44:44Z- AHRS_M2 msg: 1730868284.85 epoch sec; seqNo:1300 2024-11-06T04:44:44Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.48 psi 2024-11-06T04:44:44Z- handleDepth()- Depth_Keller msg: 1730868284.87 epoch sec; seqNo:1405; depth 50.35 2024-11-06T04:44:44Z- handleDVL() - DVL msg epochMillisec: 1730868284.88 2024-11-06T04:44:44Z- handleDvl() - RDI_Pathfinder msg: 1730868284.88 sec; seqNo:478; #beams:4 2024-11-06T04:44:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.53 , 28.82 , 30.99 , 32.08 2024-11-06T04:44:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.09 , -0.02 2024-11-06T04:44:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868284.90 epoch sec; seqNo:1405; 4061678.92 north; 594397.74 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:44:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868281.24 2024-11-06T04:44:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:45Z- waiting for 1730868285.24, time is 1730868284.90 2024-11-06T04:44:45Z- AHRS_M2 msg: 1730868285.25 epoch sec; seqNo:1301 2024-11-06T04:44:45Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.48 psi 2024-11-06T04:44:45Z- handleDepth()- Depth_Keller msg: 1730868285.26 epoch sec; seqNo:1406; depth 50.36 2024-11-06T04:44:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868285.27 epoch sec; seqNo:1406; 4061678.92 north; 594397.74 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:44:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868281.24 2024-11-06T04:44:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:45Z- waiting for 1730868285.24, time is 1730868285.27 2024-11-06T04:44:45Z- LcmTrn::updateTrn() >>>> pose time 1730868285.27, meas time 1730868284.88 <<<< 2024-11-06T04:44:45Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868285.27 2024-11-06T04:44:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868284.88 sec, ping # 0; waiting for INS data more recent than 1730868281.24... 2024-11-06T04:44:45Z- TerrainNav::motionUpdate() - currEstimate = (4061679, 594397.74, 50.36) 2024-11-06T04:44:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:44:45Z- TerrainNav:: SubcloudNIS is 0.57, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:44:45Z- TNavPF::Projecting Measurements 2024-11-06T04:44:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:44:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:44:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:44:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:44:45Z- TNavFilter::4 good beams. 2024-11-06T04:44:45Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:44:45Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:44:45Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:44:45Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:44:45Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:44:45Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:44:45Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:44:45Z- TerrainMapOctree: (vehN, vehE ) = (4061678.84, 594397.51). 2024-11-06T04:44:45Z- TerrainMapOctree: Min Distance = 163.78. 2024-11-06T04:44:45Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:44:45Z- Forcing Subcloud Comparison 2024-11-06T04:44:45Z- Weighting particles with subcloud comparison 2024-11-06T04:44:45Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:45Z- meanExpectedMeasDiff: -1.546757 delta_rms_squared: 1.367298 alpha: 0.985583 2024-11-06T04:44:45Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:45Z- meanExpectedMeasDiff: -0.159082 delta_rms_squared: 1.053684 alpha: 0.981373 2024-11-06T04:44:45Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:45Z- meanExpectedMeasDiff: 1.492981 delta_rms_squared: 1.052217 alpha: 0.981347 2024-11-06T04:44:45Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:45Z- meanExpectedMeasDiff: 0.520245 delta_rms_squared: 1.152470 alpha: 0.982942 2024-11-06T04:44:45Z- Subcloud NIS: 0.586859 2024-11-06T04:44:45Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:44:45Z- TNavPF:: sumWeights = 0.305884 2024-11-06T04:44:45Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:44:45Z- TNavPF::First Term of Map Covariance Matrix is 0.54 2024-11-06T04:44:45Z- TNavPF::Calculated NIS Value : 0.19 numBeams normalized NIS: 0.05 2024-11-06T04:44:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868284.88 sec. 2024-11-06T04:44:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:44:45Z- variances([0], [2], and psi_berg) = 0.259273 0.351296 0.000000 2024-11-06T04:44:45Z- publishEstimatesDecoder() - at 1730868285.27 sec 2024-11-06T04:44:45Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:44:45Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061622.99 594401.05 55.54 2024-11-06T04:44:45Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061622.46 594401.59 55.63 lat/lon: 36.70 -121.94 2024-11-06T04:44:45Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.35 0.63 0.00 2024-11-06T04:44:45Z- AHRS_M2 msg: 1730868285.66 epoch sec; seqNo:1302 2024-11-06T04:44:45Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.48 psi 2024-11-06T04:44:45Z- handleDepth()- Depth_Keller msg: 1730868285.68 epoch sec; seqNo:1407; depth 50.36 2024-11-06T04:44:45Z- handleDVL() - DVL msg epochMillisec: 1730868285.72 2024-11-06T04:44:45Z- handleDvl() - RDI_Pathfinder msg: 1730868285.72 sec; seqNo:479; #beams:4 2024-11-06T04:44:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.81 , 29.05 , 30.89 , 30.58 2024-11-06T04:44:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.07 , 0.01 2024-11-06T04:44:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868285.76 epoch sec; seqNo:1407; 4061678.93 north; 594398.96 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:44:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868285.27 2024-11-06T04:44:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:45Z- waiting for 1730868289.27, time is 1730868285.76 2024-11-06T04:44:46Z- AHRS_M2 msg: 1730868286.06 epoch sec; seqNo:1303 2024-11-06T04:44:46Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.47 psi 2024-11-06T04:44:46Z- handleDepth()- Depth_Keller msg: 1730868286.06 epoch sec; seqNo:1408; depth 50.36 2024-11-06T04:44:46Z- handleDVL() - DVL msg epochMillisec: 1730868286.07 2024-11-06T04:44:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:44:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:44:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:44:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:44:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:44:46Z- handleDvl() - RDI_Pathfinder msg: 1730868286.07 sec; seqNo:480; #beams:0 2024-11-06T04:44:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:44:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.07 , 0.01 2024-11-06T04:44:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868286.09 epoch sec; seqNo:1408; 4061678.93 north; 594398.96 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:44:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868285.27 2024-11-06T04:44:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:46Z- waiting for 1730868289.27, time is 1730868286.09 2024-11-06T04:44:46Z- AHRS_M2 msg: 1730868286.46 epoch sec; seqNo:1304 2024-11-06T04:44:46Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.47 psi 2024-11-06T04:44:46Z- handleDepth()- Depth_Keller msg: 1730868286.47 epoch sec; seqNo:1409; depth 50.36 2024-11-06T04:44:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868286.48 epoch sec; seqNo:1409; 4061678.93 north; 594398.96 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:44:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868285.27 2024-11-06T04:44:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:46Z- waiting for 1730868289.27, time is 1730868286.48 2024-11-06T04:44:46Z- AHRS_M2 msg: 1730868286.87 epoch sec; seqNo:1305 2024-11-06T04:44:46Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.47 psi 2024-11-06T04:44:46Z- handleDepth()- Depth_Keller msg: 1730868286.88 epoch sec; seqNo:1410; depth 50.38 2024-11-06T04:44:46Z- handleDVL() - DVL msg epochMillisec: 1730868286.90 2024-11-06T04:44:46Z- handleDvl() - RDI_Pathfinder msg: 1730868286.90 sec; seqNo:481; #beams:4 2024-11-06T04:44:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.67 , 29.36 , 30.72 , 30.45 2024-11-06T04:44:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , 0.05 2024-11-06T04:44:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868286.91 epoch sec; seqNo:1410; 4061678.93 north; 594398.96 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:44:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868285.27 2024-11-06T04:44:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:47Z- waiting for 1730868289.27, time is 1730868286.91 2024-11-06T04:44:47Z- AHRS_M2 msg: 1730868287.27 epoch sec; seqNo:1306 2024-11-06T04:44:47Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.47 psi 2024-11-06T04:44:47Z- handleDepth()- Depth_Keller msg: 1730868287.28 epoch sec; seqNo:1411; depth 50.40 2024-11-06T04:44:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868287.30 epoch sec; seqNo:1411; 4061679.32 north; 594400.18 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T04:44:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868285.27 2024-11-06T04:44:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:47Z- waiting for 1730868289.27, time is 1730868287.30 2024-11-06T04:44:47Z- AHRS_M2 msg: 1730868287.68 epoch sec; seqNo:1307 2024-11-06T04:44:47Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.46 psi 2024-11-06T04:44:47Z- handleDepth()- Depth_Keller msg: 1730868287.68 epoch sec; seqNo:1412; depth 50.40 2024-11-06T04:44:47Z- handleDVL() - DVL msg epochMillisec: 1730868287.72 2024-11-06T04:44:47Z- handleDvl() - RDI_Pathfinder msg: 1730868287.72 sec; seqNo:482; #beams:4 2024-11-06T04:44:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.94 , 28.82 , 30.99 , 30.45 2024-11-06T04:44:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.06 2024-11-06T04:44:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868287.73 epoch sec; seqNo:1412; 4061679.32 north; 594400.18 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T04:44:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868285.27 2024-11-06T04:44:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:47Z- waiting for 1730868289.27, time is 1730868287.73 2024-11-06T04:44:48Z- AHRS_M2 msg: 1730868288.08 epoch sec; seqNo:1308 2024-11-06T04:44:48Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.46 psi 2024-11-06T04:44:48Z- handleDepth()- Depth_Keller msg: 1730868288.09 epoch sec; seqNo:1413; depth 50.42 2024-11-06T04:44:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868288.10 epoch sec; seqNo:1413; 4061679.32 north; 594400.18 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T04:44:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868285.27 2024-11-06T04:44:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:48Z- waiting for 1730868289.27, time is 1730868288.10 2024-11-06T04:44:48Z- AHRS_M2 msg: 1730868288.48 epoch sec; seqNo:1309 2024-11-06T04:44:48Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.46 psi 2024-11-06T04:44:48Z- handleDepth()- Depth_Keller msg: 1730868288.49 epoch sec; seqNo:1414; depth 50.42 2024-11-06T04:44:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868288.50 epoch sec; seqNo:1414; 4061679.34 north; 594401.40 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T04:44:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868285.27 2024-11-06T04:44:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:48Z- waiting for 1730868289.27, time is 1730868288.50 2024-11-06T04:44:48Z- handleDepth()- Depth_Keller msg: 1730868288.91 epoch sec; seqNo:1415; depth 50.42 2024-11-06T04:44:48Z- handleDVL() - DVL msg epochMillisec: 1730868288.93 2024-11-06T04:44:48Z- handleDvl() - RDI_Pathfinder msg: 1730868288.93 sec; seqNo:483; #beams:4 2024-11-06T04:44:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.81 , 29.09 , 31.26 , 30.31 2024-11-06T04:44:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.04 , 0.06 2024-11-06T04:44:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868288.94 epoch sec; seqNo:1415; 4061679.34 north; 594401.40 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T04:44:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868285.27 2024-11-06T04:44:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:49Z- waiting for 1730868289.27, time is 1730868288.94 2024-11-06T04:44:49Z- AHRS_M2 msg: 1730868289.29 epoch sec; seqNo:1310 2024-11-06T04:44:49Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.46 psi 2024-11-06T04:44:49Z- handleDepth()- Depth_Keller msg: 1730868289.30 epoch sec; seqNo:1416; depth 50.42 2024-11-06T04:44:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868289.31 epoch sec; seqNo:1416; 4061679.34 north; 594401.40 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T04:44:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868285.27 2024-11-06T04:44:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:49Z- waiting for 1730868289.27, time is 1730868289.31 2024-11-06T04:44:49Z- LcmTrn::updateTrn() >>>> pose time 1730868289.31, meas time 1730868288.93 <<<< 2024-11-06T04:44:49Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868289.31 2024-11-06T04:44:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868288.93 sec, ping # 0; waiting for INS data more recent than 1730868285.27... 2024-11-06T04:44:49Z- TerrainNav::motionUpdate() - currEstimate = (4061679, 594401.40, 50.42) 2024-11-06T04:44:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:44:49Z- TerrainNav:: SubcloudNIS is 0.59, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:44:49Z- TNavPF::Projecting Measurements 2024-11-06T04:44:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:44:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:44:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:44:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:44:49Z- TNavFilter::4 good beams. 2024-11-06T04:44:49Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:44:49Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:44:49Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:44:49Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:44:49Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:44:49Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:44:49Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:44:49Z- TerrainMapOctree: (vehN, vehE ) = (4061679.30, 594401.06). 2024-11-06T04:44:49Z- TerrainMapOctree: Min Distance = 166.78. 2024-11-06T04:44:49Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:44:49Z- Forcing Subcloud Comparison 2024-11-06T04:44:49Z- Weighting particles with subcloud comparison 2024-11-06T04:44:49Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:49Z- meanExpectedMeasDiff: -2.358860 delta_rms_squared: 1.404839 alpha: 0.985963 2024-11-06T04:44:49Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:49Z- meanExpectedMeasDiff: 1.129761 delta_rms_squared: 0.938085 alpha: 0.979125 2024-11-06T04:44:49Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:49Z- meanExpectedMeasDiff: 1.862014 delta_rms_squared: 1.211624 alpha: 0.983761 2024-11-06T04:44:49Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:49Z- meanExpectedMeasDiff: -0.860896 delta_rms_squared: 1.167232 alpha: 0.983154 2024-11-06T04:44:49Z- Subcloud NIS: 0.605853 2024-11-06T04:44:49Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:44:49Z- TNavPF:: sumWeights = 0.123992 2024-11-06T04:44:49Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:44:49Z- TNavPF::First Term of Map Covariance Matrix is 0.56 2024-11-06T04:44:49Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.02 2024-11-06T04:44:49Z- TerrainNav::Resampling particle filter... 2024-11-06T04:44:49Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868288.93 sec. 2024-11-06T04:44:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:44:49Z- variances([0], [2], and psi_berg) = 0.264209 0.341831 0.000000 2024-11-06T04:44:49Z- publishEstimatesDecoder() - at 1730868289.31 sec 2024-11-06T04:44:49Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:44:49Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061623.49 594406.14 54.88 2024-11-06T04:44:49Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061622.89 594405.21 55.77 lat/lon: 36.70 -121.94 2024-11-06T04:44:49Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.34 0.60 0.00 2024-11-06T04:44:49Z- AHRS_M2 msg: 1730868289.71 epoch sec; seqNo:1311 2024-11-06T04:44:49Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.46 psi 2024-11-06T04:44:49Z- handleDepth()- Depth_Keller msg: 1730868289.71 epoch sec; seqNo:1417; depth 50.43 2024-11-06T04:44:49Z- handleDVL() - DVL msg epochMillisec: 1730868289.76 2024-11-06T04:44:49Z- handleDvl() - RDI_Pathfinder msg: 1730868289.76 sec; seqNo:484; #beams:4 2024-11-06T04:44:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.08 , 28.68 , 31.67 , 30.45 2024-11-06T04:44:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.04 2024-11-06T04:44:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868289.79 epoch sec; seqNo:1417; 4061679.34 north; 594401.40 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:44:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868289.31 2024-11-06T04:44:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:49Z- waiting for 1730868293.31, time is 1730868289.79 2024-11-06T04:44:50Z- AHRS_M2 msg: 1730868290.10 epoch sec; seqNo:1312 2024-11-06T04:44:50Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.46 psi 2024-11-06T04:44:50Z- handleDepth()- Depth_Keller msg: 1730868290.11 epoch sec; seqNo:1418; depth 50.43 2024-11-06T04:44:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868290.13 epoch sec; seqNo:1418; 4061679.73 north; 594402.62 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:44:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868289.31 2024-11-06T04:44:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:50Z- waiting for 1730868293.31, time is 1730868290.13 2024-11-06T04:44:50Z- AHRS_M2 msg: 1730868290.55 epoch sec; seqNo:1313 2024-11-06T04:44:50Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.47 psi 2024-11-06T04:44:50Z- handleDepth()- Depth_Keller msg: 1730868290.55 epoch sec; seqNo:1419; depth 50.42 2024-11-06T04:44:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868290.56 epoch sec; seqNo:1419; 4061679.73 north; 594402.62 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T04:44:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868289.31 2024-11-06T04:44:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:50Z- waiting for 1730868293.31, time is 1730868290.56 2024-11-06T04:44:50Z- AHRS_M2 msg: 1730868290.91 epoch sec; seqNo:1314 2024-11-06T04:44:50Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.47 psi 2024-11-06T04:44:50Z- handleDepth()- Depth_Keller msg: 1730868290.92 epoch sec; seqNo:1420; depth 50.43 2024-11-06T04:44:50Z- handleDVL() - DVL msg epochMillisec: 1730868290.94 2024-11-06T04:44:50Z- handleDvl() - RDI_Pathfinder msg: 1730868290.94 sec; seqNo:485; #beams:4 2024-11-06T04:44:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.08 , 28.82 , 31.81 , 30.45 2024-11-06T04:44:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.01 , 0.02 2024-11-06T04:44:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868290.96 epoch sec; seqNo:1420; 4061679.73 north; 594402.62 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:44:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868289.31 2024-11-06T04:44:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:51Z- waiting for 1730868293.31, time is 1730868290.96 2024-11-06T04:44:51Z- AHRS_M2 msg: 1730868291.32 epoch sec; seqNo:1315 2024-11-06T04:44:51Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.47 psi 2024-11-06T04:44:51Z- handleDepth()- Depth_Keller msg: 1730868291.33 epoch sec; seqNo:1421; depth 50.42 2024-11-06T04:44:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868291.35 epoch sec; seqNo:1421; 4061679.74 north; 594403.84 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T04:44:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868289.31 2024-11-06T04:44:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:51Z- waiting for 1730868293.31, time is 1730868291.35 2024-11-06T04:44:51Z- AHRS_M2 msg: 1730868291.73 epoch sec; seqNo:1316 2024-11-06T04:44:51Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.47 psi 2024-11-06T04:44:51Z- handleDepth()- Depth_Keller msg: 1730868291.73 epoch sec; seqNo:1422; depth 50.41 2024-11-06T04:44:51Z- handleDVL() - DVL msg epochMillisec: 1730868291.76 2024-11-06T04:44:51Z- handleDvl() - RDI_Pathfinder msg: 1730868291.76 sec; seqNo:486; #beams:4 2024-11-06T04:44:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.49 , 28.68 , 32.08 , 29.90 2024-11-06T04:44:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.00 , -0.01 2024-11-06T04:44:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868291.77 epoch sec; seqNo:1422; 4061679.74 north; 594403.84 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T04:44:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868289.31 2024-11-06T04:44:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:51Z- waiting for 1730868293.31, time is 1730868291.77 2024-11-06T04:44:52Z- AHRS_M2 msg: 1730868292.12 epoch sec; seqNo:1317 2024-11-06T04:44:52Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.48 psi 2024-11-06T04:44:52Z- handleDepth()- Depth_Keller msg: 1730868292.13 epoch sec; seqNo:1423; depth 50.39 2024-11-06T04:44:52Z- handleDVL() - DVL msg epochMillisec: 1730868292.13 2024-11-06T04:44:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:44:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:44:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:44:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:44:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:44:52Z- handleDvl() - RDI_Pathfinder msg: 1730868292.13 sec; seqNo:487; #beams:0 2024-11-06T04:44:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:44:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.00 , -0.01 2024-11-06T04:44:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868292.15 epoch sec; seqNo:1423; 4061679.74 north; 594403.84 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:44:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868289.31 2024-11-06T04:44:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:52Z- waiting for 1730868293.31, time is 1730868292.15 2024-11-06T04:44:52Z- AHRS_M2 msg: 1730868292.53 epoch sec; seqNo:1318 2024-11-06T04:44:52Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.48 psi 2024-11-06T04:44:52Z- handleDepth()- Depth_Keller msg: 1730868292.54 epoch sec; seqNo:1424; depth 50.39 2024-11-06T04:44:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868292.55 epoch sec; seqNo:1424; 4061679.76 north; 594405.06 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:44:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868289.31 2024-11-06T04:44:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:52Z- waiting for 1730868293.31, time is 1730868292.55 2024-11-06T04:44:52Z- AHRS_M2 msg: 1730868292.94 epoch sec; seqNo:1319 2024-11-06T04:44:52Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.48 psi 2024-11-06T04:44:52Z- handleDepth()- Depth_Keller msg: 1730868292.94 epoch sec; seqNo:1425; depth 50.36 2024-11-06T04:44:52Z- handleDVL() - DVL msg epochMillisec: 1730868292.96 2024-11-06T04:44:52Z- handleDvl() - RDI_Pathfinder msg: 1730868292.96 sec; seqNo:488; #beams:4 2024-11-06T04:44:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.30 , 28.68 , 32.35 , 29.22 2024-11-06T04:44:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , -0.04 2024-11-06T04:44:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868292.99 epoch sec; seqNo:1425; 4061679.76 north; 594405.06 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:44:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868289.31 2024-11-06T04:44:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:53Z- waiting for 1730868293.31, time is 1730868292.99 2024-11-06T04:44:53Z- AHRS_M2 msg: 1730868293.34 epoch sec; seqNo:1320 2024-11-06T04:44:53Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.48 psi 2024-11-06T04:44:53Z- handleDepth()- Depth_Keller msg: 1730868293.34 epoch sec; seqNo:1426; depth 50.34 2024-11-06T04:44:53Z- handleDVL() - DVL msg epochMillisec: 1730868293.34 2024-11-06T04:44:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:44:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:44:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:44:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:44:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:44:53Z- handleDvl() - RDI_Pathfinder msg: 1730868293.34 sec; seqNo:489; #beams:0 2024-11-06T04:44:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:44:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , -0.04 2024-11-06T04:44:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868293.36 epoch sec; seqNo:1426; 4061679.76 north; 594405.06 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:44:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868289.31 2024-11-06T04:44:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:53Z- waiting for 1730868293.31, time is 1730868293.36 2024-11-06T04:44:53Z- LcmTrn::updateTrn() >>>> pose time 1730868293.36, meas time 1730868293.34 <<<< 2024-11-06T04:44:53Z- LcmTrn::updateTrn() equating pose 1730868293.34 and meas times 1730868293.34 2024-11-06T04:44:53Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868293.34 2024-11-06T04:44:53Z- TerrainNav::motionUpdate() - currEstimate = (4061680, 594405.06, 50.34) 2024-11-06T04:44:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:44:53Z- TerrainNav:: SubcloudNIS is 0.61, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:44:53Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:44:53Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:44:53Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:44:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:44:53Z- variances([0], [2], and psi_berg) = 0.266955 0.344328 0.000000 2024-11-06T04:44:53Z- publishEstimatesDecoder() - at 1730868293.34 sec 2024-11-06T04:44:53Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:44:53Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061623.86 594409.78 54.57 2024-11-06T04:44:53Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061623.31 594408.87 55.69 lat/lon: 36.70 -121.94 2024-11-06T04:44:53Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.34 0.86 0.00 2024-11-06T04:44:53Z- AHRS_M2 msg: 1730868293.75 epoch sec; seqNo:1321 2024-11-06T04:44:53Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.49 psi 2024-11-06T04:44:53Z- handleDepth()- Depth_Keller msg: 1730868293.76 epoch sec; seqNo:1427; depth 50.33 2024-11-06T04:44:53Z- handleDVL() - DVL msg epochMillisec: 1730868293.79 2024-11-06T04:44:53Z- handleDvl() - RDI_Pathfinder msg: 1730868293.79 sec; seqNo:490; #beams:4 2024-11-06T04:44:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.33 , 28.44 , 32.57 , 29.05 2024-11-06T04:44:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , -0.07 2024-11-06T04:44:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868293.81 epoch sec; seqNo:1427; 4061680.13 north; 594405.05 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T04:44:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868293.34 2024-11-06T04:44:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:53Z- waiting for 1730868297.34, time is 1730868293.81 2024-11-06T04:44:54Z- AHRS_M2 msg: 1730868294.14 epoch sec; seqNo:1322 2024-11-06T04:44:54Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.49 psi 2024-11-06T04:44:54Z- handleDepth()- Depth_Keller msg: 1730868294.15 epoch sec; seqNo:1428; depth 50.31 2024-11-06T04:44:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868294.17 epoch sec; seqNo:1428; 4061680.15 north; 594406.27 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:44:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868293.34 2024-11-06T04:44:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:54Z- waiting for 1730868297.34, time is 1730868294.17 2024-11-06T04:44:54Z- AHRS_M2 msg: 1730868294.55 epoch sec; seqNo:1323 2024-11-06T04:44:54Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.49 psi 2024-11-06T04:44:54Z- handleDepth()- Depth_Keller msg: 1730868294.56 epoch sec; seqNo:1429; depth 50.28 2024-11-06T04:44:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868294.57 epoch sec; seqNo:1429; 4061680.15 north; 594406.27 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T04:44:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868293.34 2024-11-06T04:44:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:54Z- waiting for 1730868297.34, time is 1730868294.57 2024-11-06T04:44:54Z- AHRS_M2 msg: 1730868294.95 epoch sec; seqNo:1324 2024-11-06T04:44:54Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.49 psi 2024-11-06T04:44:54Z- handleDepth()- Depth_Keller msg: 1730868294.96 epoch sec; seqNo:1430; depth 50.27 2024-11-06T04:44:55Z- handleDVL() - DVL msg epochMillisec: 1730868294.99 2024-11-06T04:44:55Z- handleDvl() - RDI_Pathfinder msg: 1730868294.99 sec; seqNo:491; #beams:4 2024-11-06T04:44:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.64 , 28.44 , 32.57 , 29.21 2024-11-06T04:44:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , -0.08 2024-11-06T04:44:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868295.01 epoch sec; seqNo:1430; 4061680.15 north; 594406.27 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T04:44:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868293.34 2024-11-06T04:44:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:55Z- waiting for 1730868297.34, time is 1730868295.01 2024-11-06T04:44:55Z- AHRS_M2 msg: 1730868295.36 epoch sec; seqNo:1325 2024-11-06T04:44:55Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.50 psi 2024-11-06T04:44:55Z- handleDepth()- Depth_Keller msg: 1730868295.36 epoch sec; seqNo:1431; depth 50.23 2024-11-06T04:44:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868295.38 epoch sec; seqNo:1431; 4061680.16 north; 594407.49 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T04:44:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868293.34 2024-11-06T04:44:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:55Z- waiting for 1730868297.34, time is 1730868295.38 2024-11-06T04:44:55Z- AHRS_M2 msg: 1730868295.79 epoch sec; seqNo:1326 2024-11-06T04:44:55Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.50 psi 2024-11-06T04:44:55Z- handleDepth()- Depth_Keller msg: 1730868295.80 epoch sec; seqNo:1432; depth 50.22 2024-11-06T04:44:55Z- handleDVL() - DVL msg epochMillisec: 1730868295.85 2024-11-06T04:44:55Z- handleDvl() - RDI_Pathfinder msg: 1730868295.85 sec; seqNo:492; #beams:4 2024-11-06T04:44:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.49 , 28.59 , 32.57 , 29.21 2024-11-06T04:44:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , -0.07 2024-11-06T04:44:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868295.86 epoch sec; seqNo:1432; 4061680.16 north; 594407.49 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T04:44:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868293.34 2024-11-06T04:44:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:55Z- waiting for 1730868297.34, time is 1730868295.86 2024-11-06T04:44:56Z- AHRS_M2 msg: 1730868296.17 epoch sec; seqNo:1327 2024-11-06T04:44:56Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.50 psi 2024-11-06T04:44:56Z- handleDepth()- Depth_Keller msg: 1730868296.17 epoch sec; seqNo:1433; depth 50.20 2024-11-06T04:44:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868296.19 epoch sec; seqNo:1433; 4061680.16 north; 594407.49 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:44:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868293.34 2024-11-06T04:44:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:56Z- waiting for 1730868297.34, time is 1730868296.19 2024-11-06T04:44:56Z- AHRS_M2 msg: 1730868296.57 epoch sec; seqNo:1328 2024-11-06T04:44:56Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:44:56Z- handleDepth()- Depth_Keller msg: 1730868296.58 epoch sec; seqNo:1434; depth 50.17 2024-11-06T04:44:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868296.59 epoch sec; seqNo:1434; 4061680.16 north; 594407.49 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T04:44:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868293.34 2024-11-06T04:44:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:56Z- waiting for 1730868297.34, time is 1730868296.59 2024-11-06T04:44:56Z- AHRS_M2 msg: 1730868296.97 epoch sec; seqNo:1329 2024-11-06T04:44:56Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:44:56Z- handleDepth()- Depth_Keller msg: 1730868296.97 epoch sec; seqNo:1435; depth 50.14 2024-11-06T04:44:57Z- handleDVL() - DVL msg epochMillisec: 1730868297.01 2024-11-06T04:44:57Z- handleDvl() - RDI_Pathfinder msg: 1730868297.01 sec; seqNo:493; #beams:4 2024-11-06T04:44:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.95 , 28.44 , 32.57 , 29.51 2024-11-06T04:44:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.07 , -0.06 2024-11-06T04:44:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868297.02 epoch sec; seqNo:1435; 4061680.18 north; 594408.71 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:44:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868293.34 2024-11-06T04:44:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:57Z- waiting for 1730868297.34, time is 1730868297.02 2024-11-06T04:44:57Z- AHRS_M2 msg: 1730868297.38 epoch sec; seqNo:1330 2024-11-06T04:44:57Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:44:57Z- handleDepth()- Depth_Keller msg: 1730868297.39 epoch sec; seqNo:1436; depth 50.12 2024-11-06T04:44:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868297.40 epoch sec; seqNo:1436; 4061680.55 north; 594408.71 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T04:44:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868293.34 2024-11-06T04:44:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:57Z- waiting for 1730868297.34, time is 1730868297.40 2024-11-06T04:44:57Z- LcmTrn::updateTrn() >>>> pose time 1730868297.40, meas time 1730868297.01 <<<< 2024-11-06T04:44:57Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868297.40 2024-11-06T04:44:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868297.01 sec, ping # 0; waiting for INS data more recent than 1730868293.34... 2024-11-06T04:44:57Z- TerrainNav::motionUpdate() - currEstimate = (4061681, 594408.71, 50.12) 2024-11-06T04:44:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:44:57Z- TerrainNav:: SubcloudNIS is 0.61, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:44:57Z- TNavPF::Projecting Measurements 2024-11-06T04:44:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:44:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:44:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:44:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:44:57Z- TNavFilter::4 good beams. 2024-11-06T04:44:57Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:44:57Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:44:57Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:44:57Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:44:57Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:44:57Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:44:57Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:44:57Z- TerrainMapOctree: (vehN, vehE ) = (4061680.48, 594408.35). 2024-11-06T04:44:57Z- TerrainMapOctree: Min Distance = 172.94. 2024-11-06T04:44:57Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:44:57Z- Forcing Subcloud Comparison 2024-11-06T04:44:57Z- Weighting particles with subcloud comparison 2024-11-06T04:44:57Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:57Z- meanExpectedMeasDiff: -0.946504 delta_rms_squared: 1.822399 alpha: 0.989145 2024-11-06T04:44:57Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:57Z- meanExpectedMeasDiff: 2.492433 delta_rms_squared: 1.270442 alpha: 0.984501 2024-11-06T04:44:57Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:57Z- meanExpectedMeasDiff: 2.664077 delta_rms_squared: 1.558377 alpha: 0.987329 2024-11-06T04:44:57Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:44:57Z- meanExpectedMeasDiff: -1.771115 delta_rms_squared: 1.462918 alpha: 0.986513 2024-11-06T04:44:57Z- Subcloud NIS: 0.636661 2024-11-06T04:44:57Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:44:57Z- TNavPF:: sumWeights = 0.047368 2024-11-06T04:44:57Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:44:57Z- TNavPF::First Term of Map Covariance Matrix is 0.11 2024-11-06T04:44:57Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T04:44:57Z- TerrainNav::Resampling particle filter... 2024-11-06T04:44:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868297.01 sec. 2024-11-06T04:44:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:44:57Z- variances([0], [2], and psi_berg) = 0.267041 0.346437 0.000000 2024-11-06T04:44:57Z- publishEstimatesDecoder() - at 1730868297.40 sec 2024-11-06T04:44:57Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:44:57Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061624.62 594413.50 53.71 2024-11-06T04:44:57Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061624.11 594412.52 55.17 lat/lon: 36.70 -121.94 2024-11-06T04:44:57Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.35 0.66 0.00 2024-11-06T04:44:57Z- AHRS_M2 msg: 1730868297.83 epoch sec; seqNo:1331 2024-11-06T04:44:57Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:44:57Z- handleDepth()- Depth_Keller msg: 1730868297.84 epoch sec; seqNo:1437; depth 50.12 2024-11-06T04:44:57Z- handleDVL() - DVL msg epochMillisec: 1730868297.88 2024-11-06T04:44:57Z- handleDvl() - RDI_Pathfinder msg: 1730868297.88 sec; seqNo:494; #beams:4 2024-11-06T04:44:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.33 , 28.59 , 32.57 , 29.51 2024-11-06T04:44:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.08 , -0.04 2024-11-06T04:44:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868297.91 epoch sec; seqNo:1437; 4061680.55 north; 594408.71 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T04:44:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868297.40 2024-11-06T04:44:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:57Z- waiting for 1730868301.40, time is 1730868297.91 2024-11-06T04:44:58Z- AHRS_M2 msg: 1730868298.18 epoch sec; seqNo:1332 2024-11-06T04:44:58Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:44:58Z- handleDepth()- Depth_Keller msg: 1730868298.19 epoch sec; seqNo:1438; depth 50.10 2024-11-06T04:44:58Z- handleDVL() - DVL msg epochMillisec: 1730868298.19 2024-11-06T04:44:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:44:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:44:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:44:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:44:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:44:58Z- handleDvl() - RDI_Pathfinder msg: 1730868298.19 sec; seqNo:495; #beams:0 2024-11-06T04:44:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:44:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.08 , -0.04 2024-11-06T04:44:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868298.22 epoch sec; seqNo:1438; 4061680.55 north; 594408.71 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T04:44:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868297.40 2024-11-06T04:44:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:58Z- waiting for 1730868301.40, time is 1730868298.22 2024-11-06T04:44:58Z- AHRS_M2 msg: 1730868298.59 epoch sec; seqNo:1333 2024-11-06T04:44:58Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:44:58Z- handleDepth()- Depth_Keller msg: 1730868298.59 epoch sec; seqNo:1439; depth 50.08 2024-11-06T04:44:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868298.60 epoch sec; seqNo:1439; 4061680.57 north; 594409.93 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T04:44:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:44:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868297.40 2024-11-06T04:44:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:58Z- waiting for 1730868301.40, time is 1730868298.60 2024-11-06T04:44:59Z- AHRS_M2 msg: 1730868299.00 epoch sec; seqNo:1334 2024-11-06T04:44:59Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:44:59Z- handleDepth()- Depth_Keller msg: 1730868299.01 epoch sec; seqNo:1440; depth 50.07 2024-11-06T04:44:59Z- handleDVL() - DVL msg epochMillisec: 1730868299.03 2024-11-06T04:44:59Z- handleDvl() - RDI_Pathfinder msg: 1730868299.03 sec; seqNo:496; #beams:4 2024-11-06T04:44:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.33 , 28.44 , 32.57 , 29.51 2024-11-06T04:44:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.08 , -0.01 2024-11-06T04:44:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868299.06 epoch sec; seqNo:1440; 4061680.57 north; 594409.93 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T04:44:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868297.40 2024-11-06T04:44:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:59Z- waiting for 1730868301.40, time is 1730868299.06 2024-11-06T04:44:59Z- AHRS_M2 msg: 1730868299.39 epoch sec; seqNo:1335 2024-11-06T04:44:59Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:44:59Z- handleDepth()- Depth_Keller msg: 1730868299.40 epoch sec; seqNo:1441; depth 50.07 2024-11-06T04:44:59Z- handleDVL() - DVL msg epochMillisec: 1730868299.40 2024-11-06T04:44:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:44:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:44:59Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:44:59Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:44:59Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:44:59Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:44:59Z- handleDvl() - RDI_Pathfinder msg: 1730868299.40 sec; seqNo:497; #beams:0 2024-11-06T04:44:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:44:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.08 , -0.01 2024-11-06T04:44:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868299.43 epoch sec; seqNo:1441; 4061680.57 north; 594409.93 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T04:44:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:44:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868297.40 2024-11-06T04:44:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:44:59Z- waiting for 1730868301.40, time is 1730868299.43 2024-11-06T04:44:59Z- AHRS_M2 msg: 1730868299.84 epoch sec; seqNo:1336 2024-11-06T04:44:59Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:44:59Z- handleDepth()- Depth_Keller msg: 1730868299.84 epoch sec; seqNo:1442; depth 50.05 2024-11-06T04:44:59Z- handleDVL() - DVL msg epochMillisec: 1730868299.86 2024-11-06T04:44:59Z- handleDvl() - RDI_Pathfinder msg: 1730868299.86 sec; seqNo:498; #beams:4 2024-11-06T04:44:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.64 , 28.59 , 32.72 , 29.51 2024-11-06T04:44:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.07 , 0.01 2024-11-06T04:44:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868299.89 epoch sec; seqNo:1442; 4061680.57 north; 594409.93 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T04:45:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868297.40 2024-11-06T04:45:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:00Z- waiting for 1730868301.40, time is 1730868299.89 2024-11-06T04:45:00Z- AHRS_M2 msg: 1730868300.20 epoch sec; seqNo:1337 2024-11-06T04:45:00Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:45:00Z- handleDepth()- Depth_Keller msg: 1730868300.21 epoch sec; seqNo:1443; depth 50.05 2024-11-06T04:45:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868300.23 epoch sec; seqNo:1443; 4061680.96 north; 594411.15 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T04:45:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868297.40 2024-11-06T04:45:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:00Z- waiting for 1730868301.40, time is 1730868300.23 2024-11-06T04:45:00Z- AHRS_M2 msg: 1730868300.61 epoch sec; seqNo:1338 2024-11-06T04:45:00Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:45:00Z- handleDepth()- Depth_Keller msg: 1730868300.62 epoch sec; seqNo:1444; depth 50.03 2024-11-06T04:45:00Z- handleDVL() - DVL msg epochMillisec: 1730868300.62 2024-11-06T04:45:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:45:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:45:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:45:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:45:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:45:00Z- handleDvl() - RDI_Pathfinder msg: 1730868300.62 sec; seqNo:499; #beams:0 2024-11-06T04:45:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:45:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.07 , 0.01 2024-11-06T04:45:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868300.63 epoch sec; seqNo:1444; 4061680.96 north; 594411.15 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T04:45:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868297.40 2024-11-06T04:45:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:00Z- waiting for 1730868301.40, time is 1730868300.63 2024-11-06T04:45:01Z- AHRS_M2 msg: 1730868301.01 epoch sec; seqNo:1339 2024-11-06T04:45:01Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:45:01Z- handleDepth()- Depth_Keller msg: 1730868301.02 epoch sec; seqNo:1445; depth 50.03 2024-11-06T04:45:01Z- handleDVL() - DVL msg epochMillisec: 1730868301.05 2024-11-06T04:45:01Z- handleDvl() - RDI_Pathfinder msg: 1730868301.05 sec; seqNo:500; #beams:4 2024-11-06T04:45:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.49 , 28.75 , 32.72 , 29.51 2024-11-06T04:45:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.06 , 0.03 2024-11-06T04:45:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868301.06 epoch sec; seqNo:1445; 4061680.96 north; 594411.15 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T04:45:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868297.40 2024-11-06T04:45:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:01Z- waiting for 1730868301.40, time is 1730868301.06 2024-11-06T04:45:01Z- AHRS_M2 msg: 1730868301.43 epoch sec; seqNo:1340 2024-11-06T04:45:01Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:45:01Z- handleDepth()- Depth_Keller msg: 1730868301.43 epoch sec; seqNo:1446; depth 50.03 2024-11-06T04:45:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868301.46 epoch sec; seqNo:1446; 4061680.96 north; 594411.15 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T04:45:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868297.40 2024-11-06T04:45:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:01Z- waiting for 1730868301.40, time is 1730868301.46 2024-11-06T04:45:01Z- LcmTrn::updateTrn() >>>> pose time 1730868301.46, meas time 1730868301.05 <<<< 2024-11-06T04:45:01Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868301.46 2024-11-06T04:45:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868301.05 sec, ping # 0; waiting for INS data more recent than 1730868297.40... 2024-11-06T04:45:01Z- TerrainNav::motionUpdate() - currEstimate = (4061681, 594411.15, 50.03) 2024-11-06T04:45:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:45:01Z- TerrainNav:: SubcloudNIS is 0.64, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:45:01Z- TNavPF::Projecting Measurements 2024-11-06T04:45:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:45:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:45:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:45:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:45:01Z- TNavFilter::4 good beams. 2024-11-06T04:45:01Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:45:01Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:45:01Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:45:01Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:45:01Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:45:01Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:45:01Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:45:01Z- TerrainMapOctree: (vehN, vehE ) = (4061680.92, 594410.90). 2024-11-06T04:45:01Z- TerrainMapOctree: Min Distance = 175.10. 2024-11-06T04:45:01Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:45:01Z- Forcing Subcloud Comparison 2024-11-06T04:45:01Z- Weighting particles with subcloud comparison 2024-11-06T04:45:01Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:01Z- meanExpectedMeasDiff: -1.758778 delta_rms_squared: 1.482987 alpha: 0.986693 2024-11-06T04:45:01Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:01Z- meanExpectedMeasDiff: 2.284966 delta_rms_squared: 1.049400 alpha: 0.981298 2024-11-06T04:45:01Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:01Z- meanExpectedMeasDiff: 2.232004 delta_rms_squared: 1.289211 alpha: 0.984724 2024-11-06T04:45:01Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:01Z- meanExpectedMeasDiff: -2.099382 delta_rms_squared: 1.190968 alpha: 0.983484 2024-11-06T04:45:01Z- Subcloud NIS: 0.663896 2024-11-06T04:45:01Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:45:01Z- TNavPF:: sumWeights = 0.043903 2024-11-06T04:45:01Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:45:01Z- TNavPF::First Term of Map Covariance Matrix is 0.16 2024-11-06T04:45:01Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T04:45:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868301.05 sec. 2024-11-06T04:45:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:45:01Z- variances([0], [2], and psi_berg) = 0.271974 0.350123 0.000000 2024-11-06T04:45:01Z- publishEstimatesDecoder() - at 1730868301.46 sec 2024-11-06T04:45:01Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:45:01Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061624.50 594415.39 54.54 2024-11-06T04:45:01Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061624.51 594414.96 55.02 lat/lon: 36.70 -121.94 2024-11-06T04:45:01Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.35 0.61 0.00 2024-11-06T04:45:01Z- AHRS_M2 msg: 1730868301.87 epoch sec; seqNo:1341 2024-11-06T04:45:01Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:45:01Z- handleDepth()- Depth_Keller msg: 1730868301.89 epoch sec; seqNo:1447; depth 50.03 2024-11-06T04:45:01Z- handleDVL() - DVL msg epochMillisec: 1730868301.92 2024-11-06T04:45:01Z- handleDvl() - RDI_Pathfinder msg: 1730868301.92 sec; seqNo:501; #beams:4 2024-11-06T04:45:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.64 , 28.90 , 32.88 , 29.82 2024-11-06T04:45:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.04 , 0.05 2024-11-06T04:45:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868301.93 epoch sec; seqNo:1447; 4061680.97 north; 594412.37 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T04:45:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868301.46 2024-11-06T04:45:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:02Z- waiting for 1730868305.46, time is 1730868301.93 2024-11-06T04:45:02Z- AHRS_M2 msg: 1730868302.22 epoch sec; seqNo:1342 2024-11-06T04:45:02Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:45:02Z- handleDepth()- Depth_Keller msg: 1730868302.23 epoch sec; seqNo:1448; depth 50.03 2024-11-06T04:45:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868302.25 epoch sec; seqNo:1448; 4061680.97 north; 594412.37 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T04:45:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868301.46 2024-11-06T04:45:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:02Z- waiting for 1730868305.46, time is 1730868302.25 2024-11-06T04:45:02Z- AHRS_M2 msg: 1730868302.63 epoch sec; seqNo:1343 2024-11-06T04:45:02Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:45:02Z- handleDepth()- Depth_Keller msg: 1730868302.63 epoch sec; seqNo:1449; depth 50.02 2024-11-06T04:45:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868302.65 epoch sec; seqNo:1449; 4061680.97 north; 594412.37 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:45:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868301.46 2024-11-06T04:45:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:02Z- waiting for 1730868305.46, time is 1730868302.65 2024-11-06T04:45:03Z- AHRS_M2 msg: 1730868303.03 epoch sec; seqNo:1344 2024-11-06T04:45:03Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:45:03Z- handleDepth()- Depth_Keller msg: 1730868303.05 epoch sec; seqNo:1450; depth 50.02 2024-11-06T04:45:03Z- handleDVL() - DVL msg epochMillisec: 1730868303.07 2024-11-06T04:45:03Z- handleDvl() - RDI_Pathfinder msg: 1730868303.07 sec; seqNo:502; #beams:4 2024-11-06T04:45:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.03 , 28.90 , 32.88 , 29.97 2024-11-06T04:45:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.04 2024-11-06T04:45:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868303.09 epoch sec; seqNo:1450; 4061680.99 north; 594413.59 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:45:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868301.46 2024-11-06T04:45:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:03Z- waiting for 1730868305.46, time is 1730868303.09 2024-11-06T04:45:03Z- AHRS_M2 msg: 1730868303.44 epoch sec; seqNo:1345 2024-11-06T04:45:03Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:45:03Z- handleDepth()- Depth_Keller msg: 1730868303.44 epoch sec; seqNo:1451; depth 50.02 2024-11-06T04:45:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868303.46 epoch sec; seqNo:1451; 4061681.37 north; 594413.58 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:45:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868301.46 2024-11-06T04:45:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:03Z- waiting for 1730868305.46, time is 1730868303.46 2024-11-06T04:45:03Z- AHRS_M2 msg: 1730868303.88 epoch sec; seqNo:1346 2024-11-06T04:45:03Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:45:03Z- handleDepth()- Depth_Keller msg: 1730868303.89 epoch sec; seqNo:1452; depth 50.01 2024-11-06T04:45:03Z- handleDVL() - DVL msg epochMillisec: 1730868303.93 2024-11-06T04:45:03Z- handleDvl() - RDI_Pathfinder msg: 1730868303.93 sec; seqNo:503; #beams:4 2024-11-06T04:45:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.57 , 28.90 , 32.72 , 29.97 2024-11-06T04:45:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , 0.04 2024-11-06T04:45:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868303.94 epoch sec; seqNo:1452; 4061681.37 north; 594413.58 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:45:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868301.46 2024-11-06T04:45:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:04Z- waiting for 1730868305.46, time is 1730868303.94 2024-11-06T04:45:04Z- AHRS_M2 msg: 1730868304.24 epoch sec; seqNo:1347 2024-11-06T04:45:04Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:45:04Z- handleDepth()- Depth_Keller msg: 1730868304.25 epoch sec; seqNo:1453; depth 50.02 2024-11-06T04:45:04Z- handleDVL() - DVL msg epochMillisec: 1730868304.26 2024-11-06T04:45:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:45:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:45:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:45:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:45:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:45:04Z- handleDvl() - RDI_Pathfinder msg: 1730868304.26 sec; seqNo:504; #beams:0 2024-11-06T04:45:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:45:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.03 , 0.04 2024-11-06T04:45:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868304.29 epoch sec; seqNo:1453; 4061681.37 north; 594413.58 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:45:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868301.46 2024-11-06T04:45:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:04Z- waiting for 1730868305.46, time is 1730868304.29 2024-11-06T04:45:04Z- AHRS_M2 msg: 1730868304.65 epoch sec; seqNo:1348 2024-11-06T04:45:04Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:45:04Z- handleDepth()- Depth_Keller msg: 1730868304.66 epoch sec; seqNo:1454; depth 50.01 2024-11-06T04:45:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868304.66 epoch sec; seqNo:1454; 4061681.38 north; 594414.80 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:45:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868301.46 2024-11-06T04:45:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:04Z- waiting for 1730868305.46, time is 1730868304.66 2024-11-06T04:45:05Z- AHRS_M2 msg: 1730868305.05 epoch sec; seqNo:1349 2024-11-06T04:45:05Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:45:05Z- handleDepth()- Depth_Keller msg: 1730868305.08 epoch sec; seqNo:1455; depth 50.01 2024-11-06T04:45:05Z- handleDVL() - DVL msg epochMillisec: 1730868305.10 2024-11-06T04:45:05Z- handleDvl() - RDI_Pathfinder msg: 1730868305.10 sec; seqNo:505; #beams:4 2024-11-06T04:45:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 31.96 , 29.51 , 33.18 , 30.12 2024-11-06T04:45:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.03 2024-11-06T04:45:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868305.12 epoch sec; seqNo:1455; 4061681.38 north; 594414.80 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:45:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868301.46 2024-11-06T04:45:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:05Z- waiting for 1730868305.46, time is 1730868305.12 2024-11-06T04:45:05Z- AHRS_M2 msg: 1730868305.45 epoch sec; seqNo:1350 2024-11-06T04:45:05Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:45:05Z- handleDepth()- Depth_Keller msg: 1730868305.47 epoch sec; seqNo:1456; depth 50.00 2024-11-06T04:45:05Z- handleDVL() - DVL msg epochMillisec: 1730868305.48 2024-11-06T04:45:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:45:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:45:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:45:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:45:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:45:05Z- handleDvl() - RDI_Pathfinder msg: 1730868305.48 sec; seqNo:506; #beams:0 2024-11-06T04:45:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:45:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.03 2024-11-06T04:45:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868305.49 epoch sec; seqNo:1456; 4061681.38 north; 594414.80 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T04:45:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868301.46 2024-11-06T04:45:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:05Z- waiting for 1730868305.46, time is 1730868305.49 2024-11-06T04:45:05Z- LcmTrn::updateTrn() >>>> pose time 1730868305.49, meas time 1730868305.48 <<<< 2024-11-06T04:45:05Z- LcmTrn::updateTrn() equating pose 1730868305.48 and meas times 1730868305.48 2024-11-06T04:45:05Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868305.48 2024-11-06T04:45:05Z- TerrainNav::motionUpdate() - currEstimate = (4061681, 594414.80, 50.00) 2024-11-06T04:45:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:45:05Z- TerrainNav:: SubcloudNIS is 0.66, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:45:05Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:45:05Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:45:05Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:45:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:45:05Z- variances([0], [2], and psi_berg) = 0.273190 0.353863 0.000000 2024-11-06T04:45:05Z- publishEstimatesDecoder() - at 1730868305.48 sec 2024-11-06T04:45:05Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:45:05Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061624.89 594419.06 54.98 2024-11-06T04:45:05Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061624.93 594418.62 54.99 lat/lon: 36.70 -121.94 2024-11-06T04:45:05Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.35 0.84 0.00 2024-11-06T04:45:05Z- AHRS_M2 msg: 1730868305.86 epoch sec; seqNo:1351 2024-11-06T04:45:05Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:45:05Z- handleDepth()- Depth_Keller msg: 1730868305.88 epoch sec; seqNo:1457; depth 49.99 2024-11-06T04:45:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868305.94 epoch sec; seqNo:1457; 4061681.39 north; 594416.02 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T04:45:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868305.48 2024-11-06T04:45:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:06Z- waiting for 1730868309.48, time is 1730868305.94 2024-11-06T04:45:06Z- AHRS_M2 msg: 1730868306.27 epoch sec; seqNo:1352 2024-11-06T04:45:06Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:45:06Z- handleDepth()- Depth_Keller msg: 1730868306.27 epoch sec; seqNo:1458; depth 49.99 2024-11-06T04:45:06Z- handleDVL() - DVL msg epochMillisec: 1730868306.30 2024-11-06T04:45:06Z- handleDvl() - RDI_Pathfinder msg: 1730868306.30 sec; seqNo:507; #beams:4 2024-11-06T04:45:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.26 , 29.51 , 33.79 , 30.12 2024-11-06T04:45:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.01 2024-11-06T04:45:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868306.32 epoch sec; seqNo:1458; 4061681.39 north; 594416.02 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T04:45:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868305.48 2024-11-06T04:45:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:06Z- waiting for 1730868309.48, time is 1730868306.32 2024-11-06T04:45:06Z- AHRS_M2 msg: 1730868306.67 epoch sec; seqNo:1353 2024-11-06T04:45:06Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:45:06Z- handleDepth()- Depth_Keller msg: 1730868306.67 epoch sec; seqNo:1459; depth 49.97 2024-11-06T04:45:06Z- handleDVL() - DVL msg epochMillisec: 1730868306.67 2024-11-06T04:45:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:45:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:45:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:45:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:45:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:45:06Z- handleDvl() - RDI_Pathfinder msg: 1730868306.67 sec; seqNo:508; #beams:0 2024-11-06T04:45:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:45:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.01 2024-11-06T04:45:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868306.69 epoch sec; seqNo:1459; 4061681.39 north; 594416.02 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T04:45:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868305.48 2024-11-06T04:45:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:06Z- waiting for 1730868309.48, time is 1730868306.69 2024-11-06T04:45:07Z- AHRS_M2 msg: 1730868307.08 epoch sec; seqNo:1354 2024-11-06T04:45:07Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:45:07Z- handleDepth()- Depth_Keller msg: 1730868307.09 epoch sec; seqNo:1460; depth 49.94 2024-11-06T04:45:07Z- handleDVL() - DVL msg epochMillisec: 1730868307.11 2024-11-06T04:45:07Z- handleDvl() - RDI_Pathfinder msg: 1730868307.11 sec; seqNo:509; #beams:4 2024-11-06T04:45:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 32.72 , 29.51 , 33.64 , 29.97 2024-11-06T04:45:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.00 , -0.00 2024-11-06T04:45:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868307.12 epoch sec; seqNo:1460; 4061681.41 north; 594417.24 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T04:45:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868305.48 2024-11-06T04:45:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:07Z- waiting for 1730868309.48, time is 1730868307.12 2024-11-06T04:45:07Z- handleDepth()- Depth_Keller msg: 1730868307.50 epoch sec; seqNo:1461; depth 49.94 2024-11-06T04:45:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868307.51 epoch sec; seqNo:1461; 4061681.41 north; 594417.24 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T04:45:07Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:45:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868305.48 2024-11-06T04:45:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:07Z- waiting for 1730868309.48, time is 1730868307.51 2024-11-06T04:45:07Z- AHRS_M2 msg: 1730868307.88 epoch sec; seqNo:1355 2024-11-06T04:45:07Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:45:07Z- handleDepth()- Depth_Keller msg: 1730868307.89 epoch sec; seqNo:1462; depth 49.92 2024-11-06T04:45:07Z- handleDVL() - DVL msg epochMillisec: 1730868307.89 2024-11-06T04:45:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:45:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:45:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:45:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:45:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:45:07Z- handleDvl() - RDI_Pathfinder msg: 1730868307.89 sec; seqNo:510; #beams:0 2024-11-06T04:45:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:45:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.00 , -0.00 2024-11-06T04:45:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868307.92 epoch sec; seqNo:1462; 4061681.78 north; 594417.24 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:45:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868305.48 2024-11-06T04:45:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:08Z- waiting for 1730868309.48, time is 1730868307.92 2024-11-06T04:45:08Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:45:08Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:45:08Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:45:08Z- AHRS_M2 msg: 1730868308.28 epoch sec; seqNo:1356 2024-11-06T04:45:08Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:45:08Z- handleDepth()- Depth_Keller msg: 1730868308.32 epoch sec; seqNo:1463; depth 49.92 2024-11-06T04:45:08Z- handleDVL() - DVL msg epochMillisec: 1730868308.34 2024-11-06T04:45:08Z- handleDvl() - RDI_Pathfinder msg: 1730868308.34 sec; seqNo:511; #beams:4 2024-11-06T04:45:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.18 , 29.51 , 33.64 , 30.28 2024-11-06T04:45:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.01 2024-11-06T04:45:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868308.36 epoch sec; seqNo:1463; 4061681.78 north; 594417.24 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:45:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868305.48 2024-11-06T04:45:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:08Z- waiting for 1730868309.48, time is 1730868308.36 2024-11-06T04:45:08Z- AHRS_M2 msg: 1730868308.69 epoch sec; seqNo:1357 2024-11-06T04:45:08Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:45:08Z- handleDepth()- Depth_Keller msg: 1730868308.69 epoch sec; seqNo:1464; depth 49.89 2024-11-06T04:45:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868308.71 epoch sec; seqNo:1464; 4061681.80 north; 594418.46 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:45:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868305.48 2024-11-06T04:45:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:08Z- waiting for 1730868309.48, time is 1730868308.71 2024-11-06T04:45:09Z- AHRS_M2 msg: 1730868309.09 epoch sec; seqNo:1358 2024-11-06T04:45:09Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:45:09Z- handleDepth()- Depth_Keller msg: 1730868309.10 epoch sec; seqNo:1465; depth 49.85 2024-11-06T04:45:09Z- handleDVL() - DVL msg epochMillisec: 1730868309.12 2024-11-06T04:45:09Z- handleDvl() - RDI_Pathfinder msg: 1730868309.12 sec; seqNo:512; #beams:4 2024-11-06T04:45:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 33.03 , 29.51 , 33.79 , 29.51 2024-11-06T04:45:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.02 2024-11-06T04:45:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868309.15 epoch sec; seqNo:1465; 4061681.80 north; 594418.46 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:45:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868305.48 2024-11-06T04:45:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:09Z- waiting for 1730868309.48, time is 1730868309.15 2024-11-06T04:45:09Z- AHRS_M2 msg: 1730868309.50 epoch sec; seqNo:1359 2024-11-06T04:45:09Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.51 psi 2024-11-06T04:45:09Z- handleDepth()- Depth_Keller msg: 1730868309.51 epoch sec; seqNo:1466; depth 49.84 2024-11-06T04:45:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868309.53 epoch sec; seqNo:1466; 4061681.80 north; 594418.46 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T04:45:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868305.48 2024-11-06T04:45:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:09Z- waiting for 1730868309.48, time is 1730868309.53 2024-11-06T04:45:09Z- LcmTrn::updateTrn() >>>> pose time 1730868309.53, meas time 1730868309.12 <<<< 2024-11-06T04:45:09Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868309.53 2024-11-06T04:45:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868309.12 sec, ping # 0; waiting for INS data more recent than 1730868305.48... 2024-11-06T04:45:09Z- TerrainNav::motionUpdate() - currEstimate = (4061682, 594418.46, 49.84) 2024-11-06T04:45:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:45:09Z- TerrainNav:: SubcloudNIS is 0.66, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:45:09Z- TNavPF::Projecting Measurements 2024-11-06T04:45:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:45:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:45:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:45:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:45:09Z- TNavFilter::4 good beams. 2024-11-06T04:45:09Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:45:09Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:45:09Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:45:09Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:45:09Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:45:09Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:45:09Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:45:09Z- TerrainMapOctree: (vehN, vehE ) = (4061681.76, 594418.09). 2024-11-06T04:45:09Z- TerrainMapOctree: Min Distance = 181.41. 2024-11-06T04:45:09Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:45:09Z- Forcing Subcloud Comparison 2024-11-06T04:45:09Z- Weighting particles with subcloud comparison 2024-11-06T04:45:09Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:09Z- meanExpectedMeasDiff: -2.553757 delta_rms_squared: 1.793774 alpha: 0.988973 2024-11-06T04:45:09Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:09Z- meanExpectedMeasDiff: 2.795334 delta_rms_squared: 1.267310 alpha: 0.984464 2024-11-06T04:45:09Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:09Z- meanExpectedMeasDiff: 2.773165 delta_rms_squared: 2.483447 alpha: 0.992011 2024-11-06T04:45:09Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:09Z- meanExpectedMeasDiff: -2.266347 delta_rms_squared: 2.036935 alpha: 0.990277 2024-11-06T04:45:09Z- Subcloud NIS: 0.704852 2024-11-06T04:45:09Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:45:09Z- TNavPF:: sumWeights = 0.009641 2024-11-06T04:45:09Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:45:09Z- TNavPF::First Term of Map Covariance Matrix is 0.07 2024-11-06T04:45:09Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:45:09Z- TerrainNav::Resampling particle filter... 2024-11-06T04:45:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868309.12 sec. 2024-11-06T04:45:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:45:09Z- variances([0], [2], and psi_berg) = 0.270264 0.456454 0.000000 2024-11-06T04:45:09Z- publishEstimatesDecoder() - at 1730868309.53 sec 2024-11-06T04:45:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:45:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061625.84 594423.34 55.14 2024-11-06T04:45:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061625.42 594422.49 54.75 lat/lon: 36.70 -121.94 2024-11-06T04:45:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.46 0.61 0.00 2024-11-06T04:45:09Z- handleDepth()- Depth_Keller msg: 1730868309.92 epoch sec; seqNo:1467; depth 49.82 2024-11-06T04:45:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868309.97 epoch sec; seqNo:1467; 4061681.81 north; 594419.68 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:45:10Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:45:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868309.53 2024-11-06T04:45:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:10Z- waiting for 1730868313.53, time is 1730868309.97 2024-11-06T04:45:10Z- handleDepth()- Depth_Keller msg: 1730868310.32 epoch sec; seqNo:1468; depth 49.79 2024-11-06T04:45:10Z- handleDVL() - DVL msg epochMillisec: 1730868310.35 2024-11-06T04:45:10Z- handleDvl() - RDI_Pathfinder msg: 1730868310.35 sec; seqNo:513; #beams:4 2024-11-06T04:45:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.71 , 29.51 , 33.95 , 29.36 2024-11-06T04:45:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , -0.03 2024-11-06T04:45:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868310.36 epoch sec; seqNo:1468; 4061681.81 north; 594419.68 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:45:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868309.53 2024-11-06T04:45:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:10Z- waiting for 1730868313.53, time is 1730868310.36 2024-11-06T04:45:10Z- AHRS_M2 msg: 1730868310.71 epoch sec; seqNo:1360 2024-11-06T04:45:10Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.51 psi 2024-11-06T04:45:10Z- handleDepth()- Depth_Keller msg: 1730868310.72 epoch sec; seqNo:1469; depth 49.75 2024-11-06T04:45:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868310.74 epoch sec; seqNo:1469; 4061681.81 north; 594419.68 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:45:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868309.53 2024-11-06T04:45:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:10Z- waiting for 1730868313.53, time is 1730868310.74 2024-11-06T04:45:11Z- AHRS_M2 msg: 1730868311.11 epoch sec; seqNo:1361 2024-11-06T04:45:11Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.51 psi 2024-11-06T04:45:11Z- handleDepth()- Depth_Keller msg: 1730868311.12 epoch sec; seqNo:1470; depth 49.74 2024-11-06T04:45:11Z- handleDVL() - DVL msg epochMillisec: 1730868311.14 2024-11-06T04:45:11Z- handleDvl() - RDI_Pathfinder msg: 1730868311.14 sec; seqNo:514; #beams:4 2024-11-06T04:45:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 34.87 , 29.36 , 33.95 , 29.36 2024-11-06T04:45:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.04 2024-11-06T04:45:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868311.18 epoch sec; seqNo:1470; 4061681.81 north; 594419.68 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T04:45:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868309.53 2024-11-06T04:45:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:11Z- waiting for 1730868313.53, time is 1730868311.18 2024-11-06T04:45:11Z- AHRS_M2 msg: 1730868311.52 epoch sec; seqNo:1362 2024-11-06T04:45:11Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T04:45:11Z- handleDepth()- Depth_Keller msg: 1730868311.52 epoch sec; seqNo:1471; depth 49.73 2024-11-06T04:45:11Z- handleDVL() - DVL msg epochMillisec: 1730868311.53 2024-11-06T04:45:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:45:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:45:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:45:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:45:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:45:11Z- handleDvl() - RDI_Pathfinder msg: 1730868311.53 sec; seqNo:515; #beams:0 2024-11-06T04:45:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:45:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.04 2024-11-06T04:45:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868311.54 epoch sec; seqNo:1471; 4061681.83 north; 594420.90 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T04:45:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868309.53 2024-11-06T04:45:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:11Z- waiting for 1730868313.53, time is 1730868311.54 2024-11-06T04:45:11Z- AHRS_M2 msg: 1730868311.92 epoch sec; seqNo:1363 2024-11-06T04:45:11Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T04:45:11Z- handleDepth()- Depth_Keller msg: 1730868311.92 epoch sec; seqNo:1472; depth 49.70 2024-11-06T04:45:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868311.94 epoch sec; seqNo:1472; 4061681.83 north; 594420.90 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T04:45:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868309.53 2024-11-06T04:45:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:12Z- waiting for 1730868313.53, time is 1730868311.94 2024-11-06T04:45:12Z- AHRS_M2 msg: 1730868312.33 epoch sec; seqNo:1364 2024-11-06T04:45:12Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T04:45:12Z- handleDepth()- Depth_Keller msg: 1730868312.33 epoch sec; seqNo:1473; depth 49.68 2024-11-06T04:45:12Z- handleDVL() - DVL msg epochMillisec: 1730868312.37 2024-11-06T04:45:12Z- handleDvl() - RDI_Pathfinder msg: 1730868312.37 sec; seqNo:516; #beams:4 2024-11-06T04:45:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.39 , 29.36 , 33.64 , 27.68 2024-11-06T04:45:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , -0.05 2024-11-06T04:45:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868312.38 epoch sec; seqNo:1473; 4061681.83 north; 594420.90 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T04:45:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868309.53 2024-11-06T04:45:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:12Z- waiting for 1730868313.53, time is 1730868312.38 2024-11-06T04:45:12Z- AHRS_M2 msg: 1730868312.73 epoch sec; seqNo:1365 2024-11-06T04:45:12Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T04:45:12Z- handleDepth()- Depth_Keller msg: 1730868312.73 epoch sec; seqNo:1474; depth 49.66 2024-11-06T04:45:12Z- handleDVL() - DVL msg epochMillisec: 1730868312.74 2024-11-06T04:45:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:45:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:45:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:45:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:45:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:45:12Z- handleDvl() - RDI_Pathfinder msg: 1730868312.74 sec; seqNo:517; #beams:0 2024-11-06T04:45:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:45:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , -0.05 2024-11-06T04:45:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868312.75 epoch sec; seqNo:1474; 4061681.84 north; 594422.12 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:45:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868309.53 2024-11-06T04:45:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:12Z- waiting for 1730868313.53, time is 1730868312.75 2024-11-06T04:45:13Z- AHRS_M2 msg: 1730868313.14 epoch sec; seqNo:1366 2024-11-06T04:45:13Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.51 psi 2024-11-06T04:45:13Z- handleDepth()- Depth_Keller msg: 1730868313.15 epoch sec; seqNo:1475; depth 49.63 2024-11-06T04:45:13Z- handleDVL() - DVL msg epochMillisec: 1730868313.17 2024-11-06T04:45:13Z- handleDvl() - RDI_Pathfinder msg: 1730868313.17 sec; seqNo:518; #beams:4 2024-11-06T04:45:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.39 , 29.05 , 33.49 , 27.83 2024-11-06T04:45:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.07 , -0.04 2024-11-06T04:45:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868313.18 epoch sec; seqNo:1475; 4061682.22 north; 594422.12 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:45:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868309.53 2024-11-06T04:45:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:13Z- waiting for 1730868313.53, time is 1730868313.18 2024-11-06T04:45:13Z- AHRS_M2 msg: 1730868313.54 epoch sec; seqNo:1367 2024-11-06T04:45:13Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.51 psi 2024-11-06T04:45:13Z- handleDepth()- Depth_Keller msg: 1730868313.55 epoch sec; seqNo:1476; depth 49.62 2024-11-06T04:45:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868313.57 epoch sec; seqNo:1476; 4061682.22 north; 594422.12 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:45:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868309.53 2024-11-06T04:45:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:13Z- waiting for 1730868313.53, time is 1730868313.57 2024-11-06T04:45:13Z- LcmTrn::updateTrn() >>>> pose time 1730868313.57, meas time 1730868313.17 <<<< 2024-11-06T04:45:13Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868313.57 2024-11-06T04:45:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868313.17 sec, ping # 0; waiting for INS data more recent than 1730868309.53... 2024-11-06T04:45:13Z- TerrainNav::motionUpdate() - currEstimate = (4061682, 594422.12, 49.62) 2024-11-06T04:45:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:45:13Z- TerrainNav:: SubcloudNIS is 0.70, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:45:13Z- TNavPF::Projecting Measurements 2024-11-06T04:45:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:45:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:45:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:45:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:45:13Z- TNavFilter::4 good beams. 2024-11-06T04:45:13Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:45:13Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:45:13Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:45:13Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:45:13Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:45:13Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:45:13Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:45:13Z- TerrainMapOctree: (vehN, vehE ) = (4061682.18, 594421.75). 2024-11-06T04:45:13Z- TerrainMapOctree: Min Distance = 184.66. 2024-11-06T04:45:13Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:45:13Z- Forcing Subcloud Comparison 2024-11-06T04:45:13Z- Weighting particles with subcloud comparison 2024-11-06T04:45:13Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:13Z- meanExpectedMeasDiff: 0.619318 delta_rms_squared: 1.391726 alpha: 0.985833 2024-11-06T04:45:13Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:13Z- meanExpectedMeasDiff: 1.949325 delta_rms_squared: 0.998104 alpha: 0.980356 2024-11-06T04:45:13Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:13Z- meanExpectedMeasDiff: 1.404989 delta_rms_squared: 1.208250 alpha: 0.983717 2024-11-06T04:45:13Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:13Z- meanExpectedMeasDiff: -3.429435 delta_rms_squared: 1.141600 alpha: 0.982782 2024-11-06T04:45:13Z- Subcloud NIS: 0.722691 2024-11-06T04:45:13Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:45:13Z- TNavPF:: sumWeights = 0.036520 2024-11-06T04:45:13Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:45:13Z- TNavPF::First Term of Map Covariance Matrix is 0.03 2024-11-06T04:45:13Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T04:45:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868313.17 sec. 2024-11-06T04:45:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:45:13Z- variances([0], [2], and psi_berg) = 0.271890 0.459684 0.000000 2024-11-06T04:45:13Z- publishEstimatesDecoder() - at 1730868313.57 sec 2024-11-06T04:45:13Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:45:13Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061625.90 594426.72 54.15 2024-11-06T04:45:13Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061625.83 594426.16 54.51 lat/lon: 36.70 -121.94 2024-11-06T04:45:13Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.46 0.61 0.00 2024-11-06T04:45:13Z- AHRS_M2 msg: 1730868313.94 epoch sec; seqNo:1368 2024-11-06T04:45:13Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.51 psi 2024-11-06T04:45:13Z- handleDepth()- Depth_Keller msg: 1730868313.95 epoch sec; seqNo:1477; depth 49.60 2024-11-06T04:45:13Z- handleDVL() - DVL msg epochMillisec: 1730868313.95 2024-11-06T04:45:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:45:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:45:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:45:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:45:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:45:13Z- handleDvl() - RDI_Pathfinder msg: 1730868313.95 sec; seqNo:519; #beams:0 2024-11-06T04:45:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:45:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.07 , -0.04 2024-11-06T04:45:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868313.96 epoch sec; seqNo:1477; 4061682.22 north; 594422.12 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:45:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868313.57 2024-11-06T04:45:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:14Z- waiting for 1730868317.57, time is 1730868313.96 2024-11-06T04:45:14Z- AHRS_M2 msg: 1730868314.34 epoch sec; seqNo:1369 2024-11-06T04:45:14Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.51 psi 2024-11-06T04:45:14Z- handleDepth()- Depth_Keller msg: 1730868314.35 epoch sec; seqNo:1478; depth 49.57 2024-11-06T04:45:14Z- handleDVL() - DVL msg epochMillisec: 1730868314.37 2024-11-06T04:45:14Z- handleDvl() - RDI_Pathfinder msg: 1730868314.37 sec; seqNo:520; #beams:4 2024-11-06T04:45:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.70 , 29.36 , 33.33 , 28.14 2024-11-06T04:45:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.08 , -0.03 2024-11-06T04:45:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868314.41 epoch sec; seqNo:1478; 4061682.23 north; 594423.34 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T04:45:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868313.57 2024-11-06T04:45:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:14Z- waiting for 1730868317.57, time is 1730868314.41 2024-11-06T04:45:14Z- AHRS_M2 msg: 1730868314.75 epoch sec; seqNo:1370 2024-11-06T04:45:14Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T04:45:14Z- handleDepth()- Depth_Keller msg: 1730868314.76 epoch sec; seqNo:1479; depth 49.55 2024-11-06T04:45:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868314.77 epoch sec; seqNo:1479; 4061682.23 north; 594423.34 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T04:45:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868313.57 2024-11-06T04:45:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:14Z- waiting for 1730868317.57, time is 1730868314.77 2024-11-06T04:45:15Z- AHRS_M2 msg: 1730868315.15 epoch sec; seqNo:1371 2024-11-06T04:45:15Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T04:45:15Z- handleDepth()- Depth_Keller msg: 1730868315.16 epoch sec; seqNo:1480; depth 49.55 2024-11-06T04:45:15Z- handleDVL() - DVL msg epochMillisec: 1730868315.19 2024-11-06T04:45:15Z- handleDvl() - RDI_Pathfinder msg: 1730868315.19 sec; seqNo:521; #beams:4 2024-11-06T04:45:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.55 , 29.51 , 33.33 , 27.98 2024-11-06T04:45:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.09 , -0.01 2024-11-06T04:45:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868315.20 epoch sec; seqNo:1480; 4061682.23 north; 594423.34 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T04:45:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868313.57 2024-11-06T04:45:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:15Z- waiting for 1730868317.57, time is 1730868315.20 2024-11-06T04:45:15Z- AHRS_M2 msg: 1730868315.56 epoch sec; seqNo:1372 2024-11-06T04:45:15Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T04:45:15Z- handleDepth()- Depth_Keller msg: 1730868315.56 epoch sec; seqNo:1481; depth 49.53 2024-11-06T04:45:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868315.58 epoch sec; seqNo:1481; 4061682.23 north; 594423.34 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T04:45:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868313.57 2024-11-06T04:45:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:15Z- waiting for 1730868317.57, time is 1730868315.58 2024-11-06T04:45:15Z- AHRS_M2 msg: 1730868315.96 epoch sec; seqNo:1373 2024-11-06T04:45:15Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T04:45:15Z- handleDepth()- Depth_Keller msg: 1730868315.97 epoch sec; seqNo:1482; depth 49.52 2024-11-06T04:45:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868315.98 epoch sec; seqNo:1482; 4061682.62 north; 594424.55 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:45:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868313.57 2024-11-06T04:45:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:16Z- waiting for 1730868317.57, time is 1730868315.98 2024-11-06T04:45:16Z- AHRS_M2 msg: 1730868316.37 epoch sec; seqNo:1374 2024-11-06T04:45:16Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T04:45:16Z- handleDepth()- Depth_Keller msg: 1730868316.38 epoch sec; seqNo:1483; depth 49.53 2024-11-06T04:45:16Z- handleDVL() - DVL msg epochMillisec: 1730868316.40 2024-11-06T04:45:16Z- handleDvl() - RDI_Pathfinder msg: 1730868316.40 sec; seqNo:522; #beams:4 2024-11-06T04:45:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.63 , 29.51 , 33.03 , 28.29 2024-11-06T04:45:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.08 , 0.01 2024-11-06T04:45:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868316.41 epoch sec; seqNo:1483; 4061682.62 north; 594424.55 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T04:45:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868313.57 2024-11-06T04:45:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:16Z- waiting for 1730868317.57, time is 1730868316.41 2024-11-06T04:45:16Z- AHRS_M2 msg: 1730868316.77 epoch sec; seqNo:1375 2024-11-06T04:45:16Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.50 psi 2024-11-06T04:45:16Z- handleDepth()- Depth_Keller msg: 1730868316.77 epoch sec; seqNo:1484; depth 49.52 2024-11-06T04:45:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868316.81 epoch sec; seqNo:1484; 4061682.62 north; 594424.55 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:45:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868313.57 2024-11-06T04:45:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:16Z- waiting for 1730868317.57, time is 1730868316.81 2024-11-06T04:45:17Z- AHRS_M2 msg: 1730868317.17 epoch sec; seqNo:1376 2024-11-06T04:45:17Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.49 psi 2024-11-06T04:45:17Z- handleDepth()- Depth_Keller msg: 1730868317.18 epoch sec; seqNo:1485; depth 49.53 2024-11-06T04:45:17Z- handleDVL() - DVL msg epochMillisec: 1730868317.21 2024-11-06T04:45:17Z- handleDvl() - RDI_Pathfinder msg: 1730868317.21 sec; seqNo:523; #beams:4 2024-11-06T04:45:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.63 , 29.51 , 32.88 , 28.59 2024-11-06T04:45:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.07 , 0.03 2024-11-06T04:45:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868317.23 epoch sec; seqNo:1485; 4061682.62 north; 594424.55 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T04:45:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868313.57 2024-11-06T04:45:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:17Z- waiting for 1730868317.57, time is 1730868317.23 2024-11-06T04:45:17Z- AHRS_M2 msg: 1730868317.58 epoch sec; seqNo:1377 2024-11-06T04:45:17Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.49 psi 2024-11-06T04:45:17Z- handleDepth()- Depth_Keller msg: 1730868317.58 epoch sec; seqNo:1486; depth 49.54 2024-11-06T04:45:17Z- handleDVL() - DVL msg epochMillisec: 1730868317.58 2024-11-06T04:45:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:45:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:45:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:45:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:45:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:45:17Z- handleDvl() - RDI_Pathfinder msg: 1730868317.58 sec; seqNo:524; #beams:0 2024-11-06T04:45:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:45:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.07 , 0.03 2024-11-06T04:45:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868317.60 epoch sec; seqNo:1486; 4061682.64 north; 594425.77 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T04:45:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868313.57 2024-11-06T04:45:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:17Z- waiting for 1730868317.57, time is 1730868317.60 2024-11-06T04:45:17Z- LcmTrn::updateTrn() >>>> pose time 1730868317.60, meas time 1730868317.58 <<<< 2024-11-06T04:45:17Z- LcmTrn::updateTrn() equating pose 1730868317.58 and meas times 1730868317.58 2024-11-06T04:45:17Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868317.58 2024-11-06T04:45:17Z- TerrainNav::motionUpdate() - currEstimate = (4061683, 594425.77, 49.54) 2024-11-06T04:45:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:45:17Z- TerrainNav:: SubcloudNIS is 0.72, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:45:17Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:45:17Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:45:17Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:45:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:45:17Z- variances([0], [2], and psi_berg) = 0.274339 0.461388 0.000000 2024-11-06T04:45:17Z- publishEstimatesDecoder() - at 1730868317.58 sec 2024-11-06T04:45:17Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:45:17Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061626.35 594430.43 54.35 2024-11-06T04:45:17Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061626.25 594429.82 54.43 lat/lon: 36.70 -121.94 2024-11-06T04:45:17Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.46 0.87 0.00 2024-11-06T04:45:17Z- AHRS_M2 msg: 1730868317.98 epoch sec; seqNo:1378 2024-11-06T04:45:17Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.49 psi 2024-11-06T04:45:17Z- handleDepth()- Depth_Keller msg: 1730868317.98 epoch sec; seqNo:1487; depth 49.54 2024-11-06T04:45:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868317.99 epoch sec; seqNo:1487; 4061682.64 north; 594425.77 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T04:45:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868317.58 2024-11-06T04:45:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:18Z- waiting for 1730868321.58, time is 1730868317.99 2024-11-06T04:45:18Z- AHRS_M2 msg: 1730868318.38 epoch sec; seqNo:1379 2024-11-06T04:45:18Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.49 psi 2024-11-06T04:45:18Z- handleDepth()- Depth_Keller msg: 1730868318.39 epoch sec; seqNo:1488; depth 49.54 2024-11-06T04:45:18Z- handleDVL() - DVL msg epochMillisec: 1730868318.41 2024-11-06T04:45:18Z- handleDvl() - RDI_Pathfinder msg: 1730868318.41 sec; seqNo:525; #beams:4 2024-11-06T04:45:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.39 , 29.67 , 32.88 , 29.36 2024-11-06T04:45:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , 0.05 2024-11-06T04:45:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868318.44 epoch sec; seqNo:1488; 4061682.64 north; 594425.77 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T04:45:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868317.58 2024-11-06T04:45:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:18Z- waiting for 1730868321.58, time is 1730868318.44 2024-11-06T04:45:18Z- AHRS_M2 msg: 1730868318.79 epoch sec; seqNo:1380 2024-11-06T04:45:18Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.49 psi 2024-11-06T04:45:18Z- handleDepth()- Depth_Keller msg: 1730868318.80 epoch sec; seqNo:1489; depth 49.54 2024-11-06T04:45:18Z- handleDVL() - DVL msg epochMillisec: 1730868318.80 2024-11-06T04:45:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:45:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:45:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:45:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:45:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:45:18Z- handleDvl() - RDI_Pathfinder msg: 1730868318.80 sec; seqNo:526; #beams:0 2024-11-06T04:45:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:45:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , 0.05 2024-11-06T04:45:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868318.82 epoch sec; seqNo:1489; 4061682.64 north; 594425.77 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T04:45:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868317.58 2024-11-06T04:45:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:18Z- waiting for 1730868321.58, time is 1730868318.82 2024-11-06T04:45:19Z- AHRS_M2 msg: 1730868319.19 epoch sec; seqNo:1381 2024-11-06T04:45:19Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.50 psi 2024-11-06T04:45:19Z- handleDepth()- Depth_Keller msg: 1730868319.20 epoch sec; seqNo:1490; depth 49.56 2024-11-06T04:45:19Z- handleDVL() - DVL msg epochMillisec: 1730868319.23 2024-11-06T04:45:19Z- handleDvl() - RDI_Pathfinder msg: 1730868319.23 sec; seqNo:527; #beams:4 2024-11-06T04:45:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.16 , 29.67 , 32.88 , 29.82 2024-11-06T04:45:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.05 2024-11-06T04:45:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868319.24 epoch sec; seqNo:1490; 4061683.03 north; 594426.99 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:45:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868317.58 2024-11-06T04:45:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:19Z- waiting for 1730868321.58, time is 1730868319.24 2024-11-06T04:45:19Z- AHRS_M2 msg: 1730868319.60 epoch sec; seqNo:1382 2024-11-06T04:45:19Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.50 psi 2024-11-06T04:45:19Z- handleDepth()- Depth_Keller msg: 1730868319.60 epoch sec; seqNo:1491; depth 49.56 2024-11-06T04:45:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868319.61 epoch sec; seqNo:1491; 4061683.03 north; 594426.99 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:45:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868317.58 2024-11-06T04:45:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:19Z- waiting for 1730868321.58, time is 1730868319.61 2024-11-06T04:45:20Z- AHRS_M2 msg: 1730868320.00 epoch sec; seqNo:1383 2024-11-06T04:45:20Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T04:45:20Z- handleDepth()- Depth_Keller msg: 1730868320.01 epoch sec; seqNo:1492; depth 49.56 2024-11-06T04:45:20Z- handleDVL() - DVL msg epochMillisec: 1730868320.01 2024-11-06T04:45:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:45:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:45:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:45:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:45:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:45:20Z- handleDvl() - RDI_Pathfinder msg: 1730868320.01 sec; seqNo:528; #beams:0 2024-11-06T04:45:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:45:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.05 2024-11-06T04:45:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868320.02 epoch sec; seqNo:1492; 4061683.03 north; 594426.99 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:45:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868317.58 2024-11-06T04:45:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:20Z- waiting for 1730868321.58, time is 1730868320.02 2024-11-06T04:45:20Z- AHRS_M2 msg: 1730868320.40 epoch sec; seqNo:1384 2024-11-06T04:45:20Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:45:20Z- handleDepth()- Depth_Keller msg: 1730868320.41 epoch sec; seqNo:1493; depth 49.56 2024-11-06T04:45:20Z- handleDVL() - DVL msg epochMillisec: 1730868320.43 2024-11-06T04:45:20Z- handleDvl() - RDI_Pathfinder msg: 1730868320.43 sec; seqNo:529; #beams:4 2024-11-06T04:45:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.16 , 29.82 , 32.88 , 30.43 2024-11-06T04:45:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.05 2024-11-06T04:45:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868320.45 epoch sec; seqNo:1493; 4061683.04 north; 594428.21 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:45:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868317.58 2024-11-06T04:45:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:20Z- waiting for 1730868321.58, time is 1730868320.45 2024-11-06T04:45:20Z- AHRS_M2 msg: 1730868320.81 epoch sec; seqNo:1385 2024-11-06T04:45:20Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:45:20Z- handleDepth()- Depth_Keller msg: 1730868320.82 epoch sec; seqNo:1494; depth 49.56 2024-11-06T04:45:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868320.84 epoch sec; seqNo:1494; 4061683.04 north; 594428.21 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:45:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868317.58 2024-11-06T04:45:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:20Z- waiting for 1730868321.58, time is 1730868320.84 2024-11-06T04:45:21Z- AHRS_M2 msg: 1730868321.21 epoch sec; seqNo:1386 2024-11-06T04:45:21Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:45:21Z- handleDepth()- Depth_Keller msg: 1730868321.22 epoch sec; seqNo:1495; depth 49.57 2024-11-06T04:45:21Z- handleDVL() - DVL msg epochMillisec: 1730868321.24 2024-11-06T04:45:21Z- handleDvl() - RDI_Pathfinder msg: 1730868321.24 sec; seqNo:530; #beams:4 2024-11-06T04:45:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.77 , 29.82 , 33.18 , 31.04 2024-11-06T04:45:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , 0.03 2024-11-06T04:45:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868321.27 epoch sec; seqNo:1495; 4061683.04 north; 594428.21 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T04:45:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868317.58 2024-11-06T04:45:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:21Z- waiting for 1730868321.58, time is 1730868321.27 2024-11-06T04:45:21Z- AHRS_M2 msg: 1730868321.62 epoch sec; seqNo:1387 2024-11-06T04:45:21Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:45:21Z- handleDepth()- Depth_Keller msg: 1730868321.62 epoch sec; seqNo:1496; depth 49.57 2024-11-06T04:45:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868321.64 epoch sec; seqNo:1496; 4061683.04 north; 594428.21 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T04:45:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868317.58 2024-11-06T04:45:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:21Z- waiting for 1730868321.58, time is 1730868321.64 2024-11-06T04:45:21Z- LcmTrn::updateTrn() >>>> pose time 1730868321.64, meas time 1730868321.24 <<<< 2024-11-06T04:45:21Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868321.64 2024-11-06T04:45:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868321.24 sec, ping # 0; waiting for INS data more recent than 1730868317.58... 2024-11-06T04:45:21Z- TerrainNav::motionUpdate() - currEstimate = (4061683, 594428.21, 49.57) 2024-11-06T04:45:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:45:21Z- TerrainNav:: SubcloudNIS is 0.72, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:45:21Z- TNavPF::Projecting Measurements 2024-11-06T04:45:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:45:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:45:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:45:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:45:21Z- TNavFilter::4 good beams. 2024-11-06T04:45:21Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:45:21Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:45:21Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:45:21Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:45:21Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:45:21Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:45:21Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:45:21Z- TerrainMapOctree: (vehN, vehE ) = (4061683.00, 594427.97). 2024-11-06T04:45:21Z- TerrainMapOctree: Min Distance = 190.16. 2024-11-06T04:45:21Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:45:21Z- Forcing Subcloud Comparison 2024-11-06T04:45:21Z- Weighting particles with subcloud comparison 2024-11-06T04:45:21Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:21Z- meanExpectedMeasDiff: 1.750215 delta_rms_squared: 1.820242 alpha: 0.989132 2024-11-06T04:45:21Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:21Z- meanExpectedMeasDiff: 2.680771 delta_rms_squared: 1.288094 alpha: 0.984711 2024-11-06T04:45:21Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:21Z- meanExpectedMeasDiff: 0.772614 delta_rms_squared: 2.056879 alpha: 0.990370 2024-11-06T04:45:21Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:21Z- meanExpectedMeasDiff: -0.733291 delta_rms_squared: 2.095700 alpha: 0.990547 2024-11-06T04:45:21Z- Subcloud NIS: 0.736755 2024-11-06T04:45:21Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:45:21Z- TNavPF:: sumWeights = 0.159486 2024-11-06T04:45:21Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:45:21Z- TNavPF::First Term of Map Covariance Matrix is 0.22 2024-11-06T04:45:21Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T04:45:21Z- TerrainNav::Resampling particle filter... 2024-11-06T04:45:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868321.24 sec. 2024-11-06T04:45:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:45:21Z- variances([0], [2], and psi_berg) = 0.269658 0.477008 0.000000 2024-11-06T04:45:21Z- publishEstimatesDecoder() - at 1730868321.64 sec 2024-11-06T04:45:21Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:45:21Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061627.00 594433.03 53.65 2024-11-06T04:45:21Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061626.66 594432.27 53.90 lat/lon: 36.70 -121.94 2024-11-06T04:45:21Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.48 0.65 0.00 2024-11-06T04:45:22Z- AHRS_M2 msg: 1730868322.02 epoch sec; seqNo:1388 2024-11-06T04:45:22Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:45:22Z- handleDepth()- Depth_Keller msg: 1730868322.02 epoch sec; seqNo:1497; depth 49.58 2024-11-06T04:45:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868322.11 epoch sec; seqNo:1497; 4061683.06 north; 594429.43 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:45:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868321.64 2024-11-06T04:45:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:22Z- waiting for 1730868325.64, time is 1730868322.11 2024-11-06T04:45:22Z- AHRS_M2 msg: 1730868322.43 epoch sec; seqNo:1389 2024-11-06T04:45:22Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:45:22Z- handleDepth()- Depth_Keller msg: 1730868322.44 epoch sec; seqNo:1498; depth 49.58 2024-11-06T04:45:22Z- handleDVL() - DVL msg epochMillisec: 1730868322.47 2024-11-06T04:45:22Z- handleDvl() - RDI_Pathfinder msg: 1730868322.47 sec; seqNo:531; #beams:4 2024-11-06T04:45:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.77 , 29.67 , 33.03 , 30.89 2024-11-06T04:45:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , 0.01 2024-11-06T04:45:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868322.48 epoch sec; seqNo:1498; 4061683.06 north; 594429.43 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:45:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868321.64 2024-11-06T04:45:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:22Z- waiting for 1730868325.64, time is 1730868322.48 2024-11-06T04:45:22Z- AHRS_M2 msg: 1730868322.84 epoch sec; seqNo:1390 2024-11-06T04:45:22Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:45:22Z- handleDepth()- Depth_Keller msg: 1730868322.84 epoch sec; seqNo:1499; depth 49.58 2024-11-06T04:45:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868322.88 epoch sec; seqNo:1499; 4061683.06 north; 594429.43 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:45:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868321.64 2024-11-06T04:45:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:22Z- waiting for 1730868325.64, time is 1730868322.88 2024-11-06T04:45:23Z- AHRS_M2 msg: 1730868323.23 epoch sec; seqNo:1391 2024-11-06T04:45:23Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:45:23Z- handleDepth()- Depth_Keller msg: 1730868323.24 epoch sec; seqNo:1500; depth 49.57 2024-11-06T04:45:23Z- handleDVL() - DVL msg epochMillisec: 1730868323.26 2024-11-06T04:45:23Z- handleDvl() - RDI_Pathfinder msg: 1730868323.26 sec; seqNo:532; #beams:4 2024-11-06T04:45:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.46 , 29.67 , 33.33 , 31.35 2024-11-06T04:45:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , 0.00 2024-11-06T04:45:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868323.28 epoch sec; seqNo:1500; 4061683.45 north; 594430.65 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T04:45:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868321.64 2024-11-06T04:45:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:23Z- waiting for 1730868325.64, time is 1730868323.28 2024-11-06T04:45:23Z- AHRS_M2 msg: 1730868323.63 epoch sec; seqNo:1392 2024-11-06T04:45:23Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:45:23Z- handleDepth()- Depth_Keller msg: 1730868323.64 epoch sec; seqNo:1501; depth 49.57 2024-11-06T04:45:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868323.67 epoch sec; seqNo:1501; 4061683.45 north; 594430.65 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T04:45:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868321.64 2024-11-06T04:45:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:23Z- waiting for 1730868325.64, time is 1730868323.67 2024-11-06T04:45:24Z- AHRS_M2 msg: 1730868324.04 epoch sec; seqNo:1393 2024-11-06T04:45:24Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:45:24Z- handleDepth()- Depth_Keller msg: 1730868324.04 epoch sec; seqNo:1502; depth 49.56 2024-11-06T04:45:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868324.05 epoch sec; seqNo:1502; 4061683.45 north; 594430.65 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:45:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868321.64 2024-11-06T04:45:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:24Z- waiting for 1730868325.64, time is 1730868324.05 2024-11-06T04:45:24Z- handleDepth()- Depth_Keller msg: 1730868324.47 epoch sec; seqNo:1503; depth 49.55 2024-11-06T04:45:24Z- handleDVL() - DVL msg epochMillisec: 1730868324.49 2024-11-06T04:45:24Z- handleDvl() - RDI_Pathfinder msg: 1730868324.49 sec; seqNo:533; #beams:4 2024-11-06T04:45:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.77 , 29.67 , 33.18 , 31.50 2024-11-06T04:45:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , -0.02 2024-11-06T04:45:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868324.51 epoch sec; seqNo:1503; 4061683.46 north; 594431.87 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T04:45:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868321.64 2024-11-06T04:45:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:24Z- waiting for 1730868325.64, time is 1730868324.51 2024-11-06T04:45:24Z- AHRS_M2 msg: 1730868324.85 epoch sec; seqNo:1394 2024-11-06T04:45:24Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:45:24Z- handleDepth()- Depth_Keller msg: 1730868324.86 epoch sec; seqNo:1504; depth 49.56 2024-11-06T04:45:24Z- handleDVL() - DVL msg epochMillisec: 1730868324.86 2024-11-06T04:45:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:45:24Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:45:24Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:45:24Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:45:24Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:45:24Z- handleDvl() - RDI_Pathfinder msg: 1730868324.86 sec; seqNo:534; #beams:0 2024-11-06T04:45:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:45:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , -0.02 2024-11-06T04:45:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868324.88 epoch sec; seqNo:1504; 4061683.46 north; 594431.87 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:45:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868321.64 2024-11-06T04:45:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:25Z- waiting for 1730868325.64, time is 1730868324.88 2024-11-06T04:45:25Z- AHRS_M2 msg: 1730868325.25 epoch sec; seqNo:1395 2024-11-06T04:45:25Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.51 psi 2024-11-06T04:45:25Z- handleDepth()- Depth_Keller msg: 1730868325.26 epoch sec; seqNo:1505; depth 49.54 2024-11-06T04:45:25Z- handleDVL() - DVL msg epochMillisec: 1730868325.28 2024-11-06T04:45:25Z- handleDvl() - RDI_Pathfinder msg: 1730868325.28 sec; seqNo:535; #beams:4 2024-11-06T04:45:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.92 , 29.67 , 33.33 , 31.65 2024-11-06T04:45:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , -0.04 2024-11-06T04:45:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868325.30 epoch sec; seqNo:1505; 4061683.46 north; 594431.87 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T04:45:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868321.64 2024-11-06T04:45:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:25Z- waiting for 1730868325.64, time is 1730868325.30 2024-11-06T04:45:25Z- AHRS_M2 msg: 1730868325.65 epoch sec; seqNo:1396 2024-11-06T04:45:25Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.51 psi 2024-11-06T04:45:25Z- handleDepth()- Depth_Keller msg: 1730868325.67 epoch sec; seqNo:1506; depth 49.52 2024-11-06T04:45:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868325.68 epoch sec; seqNo:1506; 4061683.48 north; 594433.09 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:45:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868321.64 2024-11-06T04:45:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:25Z- waiting for 1730868325.64, time is 1730868325.68 2024-11-06T04:45:25Z- LcmTrn::updateTrn() >>>> pose time 1730868325.68, meas time 1730868325.28 <<<< 2024-11-06T04:45:25Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868325.68 2024-11-06T04:45:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868325.28 sec, ping # 0; waiting for INS data more recent than 1730868321.64... 2024-11-06T04:45:25Z- TerrainNav::motionUpdate() - currEstimate = (4061683, 594433.09, 49.52) 2024-11-06T04:45:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:45:25Z- TerrainNav:: SubcloudNIS is 0.74, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:45:25Z- TNavPF::Projecting Measurements 2024-11-06T04:45:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:45:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:45:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:45:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:45:25Z- TNavFilter::4 good beams. 2024-11-06T04:45:25Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:45:25Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:45:25Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:45:25Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:45:25Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:45:25Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:45:25Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:45:25Z- TerrainMapOctree: (vehN, vehE ) = (4061683.43, 594432.60). 2024-11-06T04:45:25Z- TerrainMapOctree: Min Distance = 194.36. 2024-11-06T04:45:25Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:45:25Z- Forcing Subcloud Comparison 2024-11-06T04:45:25Z- Weighting particles with subcloud comparison 2024-11-06T04:45:25Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:25Z- meanExpectedMeasDiff: 1.127485 delta_rms_squared: 1.535251 alpha: 0.987140 2024-11-06T04:45:25Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:25Z- meanExpectedMeasDiff: 1.492409 delta_rms_squared: 0.973882 alpha: 0.979877 2024-11-06T04:45:25Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:25Z- meanExpectedMeasDiff: 0.568706 delta_rms_squared: 2.044478 alpha: 0.990312 2024-11-06T04:45:25Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:25Z- meanExpectedMeasDiff: -0.390063 delta_rms_squared: 1.953400 alpha: 0.989865 2024-11-06T04:45:25Z- Subcloud NIS: 0.718432 2024-11-06T04:45:25Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:45:25Z- TNavPF:: sumWeights = 0.372217 2024-11-06T04:45:25Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:45:25Z- TNavPF::First Term of Map Covariance Matrix is 0.31 2024-11-06T04:45:25Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T04:45:25Z- TerrainNav::Resampling particle filter... 2024-11-06T04:45:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868325.28 sec. 2024-11-06T04:45:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:45:25Z- variances([0], [2], and psi_berg) = 0.272198 0.490464 0.000000 2024-11-06T04:45:25Z- publishEstimatesDecoder() - at 1730868325.68 sec 2024-11-06T04:45:25Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:45:25Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061627.49 594438.03 53.98 2024-11-06T04:45:25Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061627.09 594437.29 53.58 lat/lon: 36.70 -121.94 2024-11-06T04:45:25Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.49 0.61 0.00 2024-11-06T04:45:26Z- AHRS_M2 msg: 1730868326.06 epoch sec; seqNo:1397 2024-11-06T04:45:26Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.51 psi 2024-11-06T04:45:26Z- handleDepth()- Depth_Keller msg: 1730868326.07 epoch sec; seqNo:1507; depth 49.52 2024-11-06T04:45:26Z- handleDVL() - DVL msg epochMillisec: 1730868326.07 2024-11-06T04:45:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:45:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:45:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:45:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:45:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:45:26Z- handleDvl() - RDI_Pathfinder msg: 1730868326.07 sec; seqNo:536; #beams:0 2024-11-06T04:45:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:45:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , -0.04 2024-11-06T04:45:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868326.09 epoch sec; seqNo:1507; 4061683.48 north; 594433.09 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:45:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868325.68 2024-11-06T04:45:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:26Z- waiting for 1730868329.68, time is 1730868326.09 2024-11-06T04:45:26Z- AHRS_M2 msg: 1730868326.46 epoch sec; seqNo:1398 2024-11-06T04:45:26Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.51 psi 2024-11-06T04:45:26Z- handleDepth()- Depth_Keller msg: 1730868326.47 epoch sec; seqNo:1508; depth 49.52 2024-11-06T04:45:26Z- handleDVL() - DVL msg epochMillisec: 1730868326.50 2024-11-06T04:45:26Z- handleDvl() - RDI_Pathfinder msg: 1730868326.50 sec; seqNo:537; #beams:4 2024-11-06T04:45:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.38 , 29.67 , 33.18 , 31.50 2024-11-06T04:45:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.07 , -0.05 2024-11-06T04:45:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868326.52 epoch sec; seqNo:1508; 4061683.48 north; 594433.09 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:45:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868325.68 2024-11-06T04:45:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:26Z- waiting for 1730868329.68, time is 1730868326.52 2024-11-06T04:45:26Z- AHRS_M2 msg: 1730868326.87 epoch sec; seqNo:1399 2024-11-06T04:45:26Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T04:45:26Z- handleDepth()- Depth_Keller msg: 1730868326.88 epoch sec; seqNo:1509; depth 49.52 2024-11-06T04:45:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868326.90 epoch sec; seqNo:1509; 4061683.48 north; 594433.09 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:45:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868325.68 2024-11-06T04:45:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:27Z- waiting for 1730868329.68, time is 1730868326.90 2024-11-06T04:45:27Z- AHRS_M2 msg: 1730868327.27 epoch sec; seqNo:1400 2024-11-06T04:45:27Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.51 psi 2024-11-06T04:45:27Z- handleDepth()- Depth_Keller msg: 1730868327.29 epoch sec; seqNo:1510; depth 49.53 2024-11-06T04:45:27Z- handleDVL() - DVL msg epochMillisec: 1730868327.31 2024-11-06T04:45:27Z- handleDvl() - RDI_Pathfinder msg: 1730868327.31 sec; seqNo:538; #beams:4 2024-11-06T04:45:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.92 , 29.67 , 33.33 , 31.65 2024-11-06T04:45:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.08 , -0.05 2024-11-06T04:45:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868327.32 epoch sec; seqNo:1510; 4061683.87 north; 594434.30 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T04:45:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868325.68 2024-11-06T04:45:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:27Z- waiting for 1730868329.68, time is 1730868327.32 2024-11-06T04:45:27Z- AHRS_M2 msg: 1730868327.68 epoch sec; seqNo:1401 2024-11-06T04:45:27Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.51 psi 2024-11-06T04:45:27Z- handleDepth()- Depth_Keller msg: 1730868327.68 epoch sec; seqNo:1511; depth 49.51 2024-11-06T04:45:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868327.69 epoch sec; seqNo:1511; 4061683.87 north; 594434.30 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T04:45:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868325.68 2024-11-06T04:45:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:27Z- waiting for 1730868329.68, time is 1730868327.69 2024-11-06T04:45:28Z- handleDepth()- Depth_Keller msg: 1730868328.10 epoch sec; seqNo:1512; depth 49.51 2024-11-06T04:45:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868328.11 epoch sec; seqNo:1512; 4061683.87 north; 594434.30 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T04:45:28Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:45:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868325.68 2024-11-06T04:45:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:28Z- waiting for 1730868329.68, time is 1730868328.11 2024-11-06T04:45:28Z- AHRS_M2 msg: 1730868328.50 epoch sec; seqNo:1402 2024-11-06T04:45:28Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T04:45:28Z- handleDepth()- Depth_Keller msg: 1730868328.51 epoch sec; seqNo:1513; depth 49.49 2024-11-06T04:45:28Z- handleDVL() - DVL msg epochMillisec: 1730868328.54 2024-11-06T04:45:28Z- handleDvl() - RDI_Pathfinder msg: 1730868328.54 sec; seqNo:539; #beams:4 2024-11-06T04:45:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.92 , 29.36 , 32.88 , 31.65 2024-11-06T04:45:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.10 , -0.05 2024-11-06T04:45:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868328.56 epoch sec; seqNo:1513; 4061683.88 north; 594435.52 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:45:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868325.68 2024-11-06T04:45:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:28Z- waiting for 1730868329.68, time is 1730868328.56 2024-11-06T04:45:28Z- AHRS_M2 msg: 1730868328.89 epoch sec; seqNo:1403 2024-11-06T04:45:28Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T04:45:28Z- handleDepth()- Depth_Keller msg: 1730868328.90 epoch sec; seqNo:1514; depth 49.50 2024-11-06T04:45:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868328.93 epoch sec; seqNo:1514; 4061683.88 north; 594435.52 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T04:45:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868325.68 2024-11-06T04:45:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:29Z- waiting for 1730868329.68, time is 1730868328.93 2024-11-06T04:45:29Z- AHRS_M2 msg: 1730868329.29 epoch sec; seqNo:1404 2024-11-06T04:45:29Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T04:45:29Z- handleDepth()- Depth_Keller msg: 1730868329.30 epoch sec; seqNo:1515; depth 49.52 2024-11-06T04:45:29Z- handleDVL() - DVL msg epochMillisec: 1730868329.32 2024-11-06T04:45:29Z- handleDvl() - RDI_Pathfinder msg: 1730868329.32 sec; seqNo:540; #beams:4 2024-11-06T04:45:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.08 , 29.21 , 32.57 , 31.81 2024-11-06T04:45:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.10 , -0.04 2024-11-06T04:45:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868329.35 epoch sec; seqNo:1515; 4061683.88 north; 594435.52 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:45:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868325.68 2024-11-06T04:45:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:29Z- waiting for 1730868329.68, time is 1730868329.35 2024-11-06T04:45:29Z- AHRS_M2 msg: 1730868329.70 epoch sec; seqNo:1405 2024-11-06T04:45:29Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T04:45:29Z- handleDepth()- Depth_Keller msg: 1730868329.71 epoch sec; seqNo:1516; depth 49.52 2024-11-06T04:45:29Z- handleDVL() - DVL msg epochMillisec: 1730868329.71 2024-11-06T04:45:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:45:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:45:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:45:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:45:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:45:29Z- handleDvl() - RDI_Pathfinder msg: 1730868329.71 sec; seqNo:541; #beams:0 2024-11-06T04:45:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:45:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.10 , -0.04 2024-11-06T04:45:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868329.73 epoch sec; seqNo:1516; 4061683.88 north; 594435.52 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:45:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868325.68 2024-11-06T04:45:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:29Z- waiting for 1730868329.68, time is 1730868329.73 2024-11-06T04:45:29Z- LcmTrn::updateTrn() >>>> pose time 1730868329.73, meas time 1730868329.71 <<<< 2024-11-06T04:45:29Z- LcmTrn::updateTrn() equating pose 1730868329.71 and meas times 1730868329.71 2024-11-06T04:45:29Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868329.71 2024-11-06T04:45:29Z- TerrainNav::motionUpdate() - currEstimate = (4061684, 594435.52, 49.52) 2024-11-06T04:45:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:45:29Z- TerrainNav:: SubcloudNIS is 0.72, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:45:29Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:45:29Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:45:29Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:45:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:45:29Z- variances([0], [2], and psi_berg) = 0.273316 0.491892 0.000000 2024-11-06T04:45:29Z- publishEstimatesDecoder() - at 1730868329.71 sec 2024-11-06T04:45:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:45:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061627.93 594440.46 54.75 2024-11-06T04:45:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061627.49 594439.73 53.57 lat/lon: 36.70 -121.94 2024-11-06T04:45:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.49 0.88 0.00 2024-11-06T04:45:30Z- AHRS_M2 msg: 1730868330.10 epoch sec; seqNo:1406 2024-11-06T04:45:30Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T04:45:30Z- handleDepth()- Depth_Keller msg: 1730868330.12 epoch sec; seqNo:1517; depth 49.53 2024-11-06T04:45:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868330.13 epoch sec; seqNo:1517; 4061684.27 north; 594436.74 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T04:45:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868329.71 2024-11-06T04:45:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:30Z- waiting for 1730868333.71, time is 1730868330.13 2024-11-06T04:45:30Z- AHRS_M2 msg: 1730868330.51 epoch sec; seqNo:1407 2024-11-06T04:45:30Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T04:45:30Z- handleDepth()- Depth_Keller msg: 1730868330.51 epoch sec; seqNo:1518; depth 49.53 2024-11-06T04:45:30Z- handleDVL() - DVL msg epochMillisec: 1730868330.57 2024-11-06T04:45:30Z- handleDvl() - RDI_Pathfinder msg: 1730868330.57 sec; seqNo:542; #beams:4 2024-11-06T04:45:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.77 , 29.21 , 32.57 , 31.96 2024-11-06T04:45:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.10 , -0.01 2024-11-06T04:45:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868330.60 epoch sec; seqNo:1518; 4061684.27 north; 594436.74 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T04:45:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868329.71 2024-11-06T04:45:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:30Z- waiting for 1730868333.71, time is 1730868330.60 2024-11-06T04:45:30Z- AHRS_M2 msg: 1730868330.91 epoch sec; seqNo:1408 2024-11-06T04:45:30Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.50 psi 2024-11-06T04:45:30Z- handleDepth()- Depth_Keller msg: 1730868330.92 epoch sec; seqNo:1519; depth 49.55 2024-11-06T04:45:30Z- handleDVL() - DVL msg epochMillisec: 1730868330.92 2024-11-06T04:45:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:45:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:45:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:45:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:45:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:45:30Z- handleDvl() - RDI_Pathfinder msg: 1730868330.92 sec; seqNo:543; #beams:0 2024-11-06T04:45:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:45:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.10 , -0.01 2024-11-06T04:45:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868330.94 epoch sec; seqNo:1519; 4061684.27 north; 594436.74 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T04:45:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868329.71 2024-11-06T04:45:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:31Z- waiting for 1730868333.71, time is 1730868330.94 2024-11-06T04:45:31Z- AHRS_M2 msg: 1730868331.31 epoch sec; seqNo:1409 2024-11-06T04:45:31Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.50 psi 2024-11-06T04:45:31Z- handleDepth()- Depth_Keller msg: 1730868331.33 epoch sec; seqNo:1520; depth 49.55 2024-11-06T04:45:31Z- handleDVL() - DVL msg epochMillisec: 1730868331.35 2024-11-06T04:45:31Z- handleDvl() - RDI_Pathfinder msg: 1730868331.35 sec; seqNo:544; #beams:4 2024-11-06T04:45:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.23 , 29.21 , 32.72 , 31.96 2024-11-06T04:45:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.08 , 0.01 2024-11-06T04:45:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868331.36 epoch sec; seqNo:1520; 4061684.27 north; 594436.74 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T04:45:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868329.71 2024-11-06T04:45:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:31Z- waiting for 1730868333.71, time is 1730868331.36 2024-11-06T04:45:31Z- AHRS_M2 msg: 1730868331.71 epoch sec; seqNo:1410 2024-11-06T04:45:31Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.50 psi 2024-11-06T04:45:31Z- handleDepth()- Depth_Keller msg: 1730868331.73 epoch sec; seqNo:1521; depth 49.58 2024-11-06T04:45:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868331.74 epoch sec; seqNo:1521; 4061684.29 north; 594437.96 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:45:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868329.71 2024-11-06T04:45:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:31Z- waiting for 1730868333.71, time is 1730868331.74 2024-11-06T04:45:32Z- AHRS_M2 msg: 1730868332.12 epoch sec; seqNo:1411 2024-11-06T04:45:32Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.50 psi 2024-11-06T04:45:32Z- handleDepth()- Depth_Keller msg: 1730868332.13 epoch sec; seqNo:1522; depth 49.59 2024-11-06T04:45:32Z- handleDVL() - DVL msg epochMillisec: 1730868332.13 2024-11-06T04:45:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:45:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:45:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:45:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:45:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:45:32Z- handleDvl() - RDI_Pathfinder msg: 1730868332.13 sec; seqNo:545; #beams:0 2024-11-06T04:45:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:45:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.08 , 0.01 2024-11-06T04:45:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868332.16 epoch sec; seqNo:1522; 4061684.29 north; 594437.96 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:45:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868329.71 2024-11-06T04:45:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:32Z- waiting for 1730868333.71, time is 1730868332.16 2024-11-06T04:45:32Z- AHRS_M2 msg: 1730868332.52 epoch sec; seqNo:1412 2024-11-06T04:45:32Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.49 psi 2024-11-06T04:45:32Z- handleDepth()- Depth_Keller msg: 1730868332.53 epoch sec; seqNo:1523; depth 49.60 2024-11-06T04:45:32Z- handleDVL() - DVL msg epochMillisec: 1730868332.55 2024-11-06T04:45:32Z- handleDvl() - RDI_Pathfinder msg: 1730868332.55 sec; seqNo:546; #beams:4 2024-11-06T04:45:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.08 , 29.05 , 32.72 , 31.96 2024-11-06T04:45:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , 0.03 2024-11-06T04:45:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868332.58 epoch sec; seqNo:1523; 4061684.67 north; 594437.95 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:45:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868329.71 2024-11-06T04:45:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:32Z- waiting for 1730868333.71, time is 1730868332.58 2024-11-06T04:45:32Z- AHRS_M2 msg: 1730868332.93 epoch sec; seqNo:1413 2024-11-06T04:45:32Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.49 psi 2024-11-06T04:45:32Z- handleDepth()- Depth_Keller msg: 1730868332.94 epoch sec; seqNo:1524; depth 49.62 2024-11-06T04:45:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868332.95 epoch sec; seqNo:1524; 4061684.67 north; 594437.95 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:45:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868329.71 2024-11-06T04:45:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:33Z- waiting for 1730868333.71, time is 1730868332.95 2024-11-06T04:45:33Z- handleDepth()- Depth_Keller msg: 1730868333.35 epoch sec; seqNo:1525; depth 49.64 2024-11-06T04:45:33Z- handleDVL() - DVL msg epochMillisec: 1730868333.37 2024-11-06T04:45:33Z- handleDvl() - RDI_Pathfinder msg: 1730868333.37 sec; seqNo:547; #beams:4 2024-11-06T04:45:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.62 , 28.90 , 32.72 , 32.26 2024-11-06T04:45:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , 0.04 2024-11-06T04:45:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868333.38 epoch sec; seqNo:1525; 4061684.68 north; 594439.18 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:45:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868329.71 2024-11-06T04:45:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:33Z- waiting for 1730868333.71, time is 1730868333.38 2024-11-06T04:45:33Z- AHRS_M2 msg: 1730868333.73 epoch sec; seqNo:1414 2024-11-06T04:45:33Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:45:33Z- handleDepth()- Depth_Keller msg: 1730868333.74 epoch sec; seqNo:1526; depth 49.65 2024-11-06T04:45:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868333.76 epoch sec; seqNo:1526; 4061684.68 north; 594439.18 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:45:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868329.71 2024-11-06T04:45:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:33Z- waiting for 1730868333.71, time is 1730868333.76 2024-11-06T04:45:33Z- LcmTrn::updateTrn() >>>> pose time 1730868333.76, meas time 1730868333.37 <<<< 2024-11-06T04:45:33Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868333.76 2024-11-06T04:45:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868333.37 sec, ping # 0; waiting for INS data more recent than 1730868329.71... 2024-11-06T04:45:33Z- TerrainNav::motionUpdate() - currEstimate = (4061685, 594439.18, 49.65) 2024-11-06T04:45:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:45:33Z- TerrainNav:: SubcloudNIS is 0.72, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:45:33Z- TNavPF::Projecting Measurements 2024-11-06T04:45:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:45:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:45:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:45:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:45:33Z- TNavFilter::4 good beams. 2024-11-06T04:45:33Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:45:33Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:45:33Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:45:33Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:45:33Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:45:33Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:45:33Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:45:33Z- TerrainMapOctree: (vehN, vehE ) = (4061684.60, 594438.82). 2024-11-06T04:45:33Z- TerrainMapOctree: Min Distance = 199.83. 2024-11-06T04:45:33Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:45:33Z- Forcing Subcloud Comparison 2024-11-06T04:45:33Z- Weighting particles with subcloud comparison 2024-11-06T04:45:33Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:33Z- meanExpectedMeasDiff: 0.178803 delta_rms_squared: 1.411046 alpha: 0.986024 2024-11-06T04:45:33Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:34Z- meanExpectedMeasDiff: -0.156645 delta_rms_squared: 1.316748 alpha: 0.985038 2024-11-06T04:45:34Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:34Z- meanExpectedMeasDiff: -2.845165 delta_rms_squared: 2.567940 alpha: 0.992272 2024-11-06T04:45:34Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:34Z- meanExpectedMeasDiff: 0.036423 delta_rms_squared: 1.627676 alpha: 0.987862 2024-11-06T04:45:34Z- Subcloud NIS: 0.685632 2024-11-06T04:45:34Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:45:34Z- TNavPF:: sumWeights = 0.241748 2024-11-06T04:45:34Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:45:34Z- TNavPF::First Term of Map Covariance Matrix is 0.19 2024-11-06T04:45:34Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T04:45:34Z- TerrainNav::Resampling particle filter... 2024-11-06T04:45:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868333.37 sec. 2024-11-06T04:45:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:45:34Z- variances([0], [2], and psi_berg) = 0.294793 0.468841 0.000000 2024-11-06T04:45:34Z- publishEstimatesDecoder() - at 1730868333.76 sec 2024-11-06T04:45:34Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:45:34Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061628.81 594444.21 55.39 2024-11-06T04:45:34Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061628.28 594443.30 54.14 lat/lon: 36.70 -121.94 2024-11-06T04:45:34Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.29 0.47 0.66 0.00 2024-11-06T04:45:34Z- AHRS_M2 msg: 1730868334.14 epoch sec; seqNo:1415 2024-11-06T04:45:34Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:45:34Z- handleDepth()- Depth_Keller msg: 1730868334.15 epoch sec; seqNo:1527; depth 49.66 2024-11-06T04:45:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868334.16 epoch sec; seqNo:1527; 4061684.68 north; 594439.18 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:45:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868333.76 2024-11-06T04:45:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:34Z- waiting for 1730868337.76, time is 1730868334.16 2024-11-06T04:45:34Z- AHRS_M2 msg: 1730868334.55 epoch sec; seqNo:1416 2024-11-06T04:45:34Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:45:34Z- handleDepth()- Depth_Keller msg: 1730868334.56 epoch sec; seqNo:1528; depth 49.68 2024-11-06T04:45:34Z- handleDVL() - DVL msg epochMillisec: 1730868334.58 2024-11-06T04:45:34Z- handleDvl() - RDI_Pathfinder msg: 1730868334.58 sec; seqNo:548; #beams:4 2024-11-06T04:45:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.08 , 29.05 , 32.72 , 32.42 2024-11-06T04:45:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , 0.04 2024-11-06T04:45:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868334.59 epoch sec; seqNo:1528; 4061684.68 north; 594439.18 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T04:45:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868333.76 2024-11-06T04:45:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:34Z- waiting for 1730868337.76, time is 1730868334.59 2024-11-06T04:45:34Z- AHRS_M2 msg: 1730868334.96 epoch sec; seqNo:1417 2024-11-06T04:45:34Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:45:34Z- handleDepth()- Depth_Keller msg: 1730868334.96 epoch sec; seqNo:1529; depth 49.71 2024-11-06T04:45:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868334.97 epoch sec; seqNo:1529; 4061684.69 north; 594440.40 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T04:45:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868333.76 2024-11-06T04:45:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:35Z- waiting for 1730868337.76, time is 1730868334.97 2024-11-06T04:45:35Z- AHRS_M2 msg: 1730868335.36 epoch sec; seqNo:1418 2024-11-06T04:45:35Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T04:45:35Z- handleDepth()- Depth_Keller msg: 1730868335.37 epoch sec; seqNo:1530; depth 49.72 2024-11-06T04:45:35Z- handleDVL() - DVL msg epochMillisec: 1730868335.39 2024-11-06T04:45:35Z- handleDvl() - RDI_Pathfinder msg: 1730868335.39 sec; seqNo:549; #beams:4 2024-11-06T04:45:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.23 , 29.21 , 33.03 , 32.57 2024-11-06T04:45:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.03 2024-11-06T04:45:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868335.41 epoch sec; seqNo:1530; 4061684.69 north; 594440.40 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:45:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868333.76 2024-11-06T04:45:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:35Z- waiting for 1730868337.76, time is 1730868335.41 2024-11-06T04:45:35Z- AHRS_M2 msg: 1730868335.76 epoch sec; seqNo:1419 2024-11-06T04:45:35Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T04:45:35Z- handleDepth()- Depth_Keller msg: 1730868335.76 epoch sec; seqNo:1531; depth 49.74 2024-11-06T04:45:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868335.78 epoch sec; seqNo:1531; 4061684.69 north; 594440.40 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T04:45:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868333.76 2024-11-06T04:45:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:35Z- waiting for 1730868337.76, time is 1730868335.78 2024-11-06T04:45:36Z- AHRS_M2 msg: 1730868336.16 epoch sec; seqNo:1420 2024-11-06T04:45:36Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T04:45:36Z- handleDepth()- Depth_Keller msg: 1730868336.16 epoch sec; seqNo:1532; depth 49.75 2024-11-06T04:45:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868336.18 epoch sec; seqNo:1532; 4061684.71 north; 594441.62 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:45:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868333.76 2024-11-06T04:45:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:36Z- waiting for 1730868337.76, time is 1730868336.18 2024-11-06T04:45:36Z- AHRS_M2 msg: 1730868336.56 epoch sec; seqNo:1421 2024-11-06T04:45:36Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T04:45:36Z- handleDepth()- Depth_Keller msg: 1730868336.57 epoch sec; seqNo:1533; depth 49.77 2024-11-06T04:45:36Z- handleDVL() - DVL msg epochMillisec: 1730868336.60 2024-11-06T04:45:36Z- handleDvl() - RDI_Pathfinder msg: 1730868336.60 sec; seqNo:550; #beams:4 2024-11-06T04:45:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.08 , 29.05 , 33.03 , 32.42 2024-11-06T04:45:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , 0.02 2024-11-06T04:45:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868336.61 epoch sec; seqNo:1533; 4061685.08 north; 594441.61 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T04:45:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868333.76 2024-11-06T04:45:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:36Z- waiting for 1730868337.76, time is 1730868336.61 2024-11-06T04:45:36Z- AHRS_M2 msg: 1730868336.97 epoch sec; seqNo:1422 2024-11-06T04:45:36Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T04:45:36Z- handleDepth()- Depth_Keller msg: 1730868336.97 epoch sec; seqNo:1534; depth 49.77 2024-11-06T04:45:36Z- handleDVL() - DVL msg epochMillisec: 1730868336.98 2024-11-06T04:45:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:45:36Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:45:36Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:45:36Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:45:36Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:45:36Z- handleDvl() - RDI_Pathfinder msg: 1730868336.98 sec; seqNo:551; #beams:0 2024-11-06T04:45:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:45:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , 0.02 2024-11-06T04:45:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868337.00 epoch sec; seqNo:1534; 4061685.08 north; 594441.61 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T04:45:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868333.76 2024-11-06T04:45:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:37Z- waiting for 1730868337.76, time is 1730868337.00 2024-11-06T04:45:37Z- AHRS_M2 msg: 1730868337.39 epoch sec; seqNo:1423 2024-11-06T04:45:37Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T04:45:37Z- handleDepth()- Depth_Keller msg: 1730868337.39 epoch sec; seqNo:1535; depth 49.79 2024-11-06T04:45:37Z- handleDVL() - DVL msg epochMillisec: 1730868337.42 2024-11-06T04:45:37Z- handleDvl() - RDI_Pathfinder msg: 1730868337.42 sec; seqNo:552; #beams:4 2024-11-06T04:45:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.92 , 29.05 , 32.88 , 32.42 2024-11-06T04:45:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.00 2024-11-06T04:45:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868337.44 epoch sec; seqNo:1535; 4061685.10 north; 594442.83 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:45:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868333.76 2024-11-06T04:45:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:37Z- waiting for 1730868337.76, time is 1730868337.44 2024-11-06T04:45:37Z- AHRS_M2 msg: 1730868337.78 epoch sec; seqNo:1424 2024-11-06T04:45:37Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:45:37Z- handleDepth()- Depth_Keller msg: 1730868337.79 epoch sec; seqNo:1536; depth 49.81 2024-11-06T04:45:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868337.80 epoch sec; seqNo:1536; 4061685.10 north; 594442.83 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:45:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868333.76 2024-11-06T04:45:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:37Z- waiting for 1730868337.76, time is 1730868337.80 2024-11-06T04:45:37Z- LcmTrn::updateTrn() >>>> pose time 1730868337.80, meas time 1730868337.42 <<<< 2024-11-06T04:45:37Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868337.80 2024-11-06T04:45:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868337.42 sec, ping # 0; waiting for INS data more recent than 1730868333.76... 2024-11-06T04:45:37Z- TerrainNav::motionUpdate() - currEstimate = (4061685, 594442.83, 49.81) 2024-11-06T04:45:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:45:37Z- TerrainNav:: SubcloudNIS is 0.69, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:45:37Z- TNavPF::Projecting Measurements 2024-11-06T04:45:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:45:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:45:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:45:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:45:37Z- TNavFilter::4 good beams. 2024-11-06T04:45:37Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:45:37Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:45:37Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:45:37Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:45:37Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:45:37Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:45:37Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:45:37Z- TerrainMapOctree: (vehN, vehE ) = (4061685.06, 594442.49). 2024-11-06T04:45:37Z- TerrainMapOctree: Min Distance = 203.15. 2024-11-06T04:45:37Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:45:38Z- Forcing Subcloud Comparison 2024-11-06T04:45:38Z- Weighting particles with subcloud comparison 2024-11-06T04:45:38Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:38Z- meanExpectedMeasDiff: 0.132310 delta_rms_squared: 1.384342 alpha: 0.985758 2024-11-06T04:45:38Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:38Z- meanExpectedMeasDiff: 0.706390 delta_rms_squared: 1.053724 alpha: 0.981373 2024-11-06T04:45:38Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:38Z- meanExpectedMeasDiff: -2.976276 delta_rms_squared: 1.195568 alpha: 0.983547 2024-11-06T04:45:38Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:38Z- meanExpectedMeasDiff: 0.787728 delta_rms_squared: 1.283201 alpha: 0.984653 2024-11-06T04:45:38Z- Subcloud NIS: 0.670514 2024-11-06T04:45:38Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:45:38Z- TNavPF:: sumWeights = 0.158510 2024-11-06T04:45:38Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:45:38Z- TNavPF::First Term of Map Covariance Matrix is 0.11 2024-11-06T04:45:38Z- TNavPF::Calculated NIS Value : 0.09 numBeams normalized NIS: 0.02 2024-11-06T04:45:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868337.42 sec. 2024-11-06T04:45:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:45:38Z- variances([0], [2], and psi_berg) = 0.297094 0.483296 0.000000 2024-11-06T04:45:38Z- publishEstimatesDecoder() - at 1730868337.80 sec 2024-11-06T04:45:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:45:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061627.63 594445.42 54.36 2024-11-06T04:45:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061628.70 594446.98 54.46 lat/lon: 36.70 -121.94 2024-11-06T04:45:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.30 0.48 0.63 0.00 2024-11-06T04:45:38Z- AHRS_M2 msg: 1730868338.19 epoch sec; seqNo:1425 2024-11-06T04:45:38Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T04:45:38Z- handleDepth()- Depth_Keller msg: 1730868338.19 epoch sec; seqNo:1537; depth 49.82 2024-11-06T04:45:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868338.20 epoch sec; seqNo:1537; 4061685.10 north; 594442.83 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:45:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868337.80 2024-11-06T04:45:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:38Z- waiting for 1730868341.80, time is 1730868338.20 2024-11-06T04:45:38Z- AHRS_M2 msg: 1730868338.58 epoch sec; seqNo:1426 2024-11-06T04:45:38Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:45:38Z- handleDepth()- Depth_Keller msg: 1730868338.59 epoch sec; seqNo:1538; depth 49.83 2024-11-06T04:45:38Z- handleDVL() - DVL msg epochMillisec: 1730868338.61 2024-11-06T04:45:38Z- handleDvl() - RDI_Pathfinder msg: 1730868338.61 sec; seqNo:553; #beams:4 2024-11-06T04:45:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.23 , 29.36 , 32.88 , 32.57 2024-11-06T04:45:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , -0.02 2024-11-06T04:45:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868338.65 epoch sec; seqNo:1538; 4061685.10 north; 594442.83 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:45:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868337.80 2024-11-06T04:45:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:38Z- waiting for 1730868341.80, time is 1730868338.65 2024-11-06T04:45:38Z- AHRS_M2 msg: 1730868338.99 epoch sec; seqNo:1427 2024-11-06T04:45:38Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:45:38Z- handleDepth()- Depth_Keller msg: 1730868338.99 epoch sec; seqNo:1539; depth 49.83 2024-11-06T04:45:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868339.01 epoch sec; seqNo:1539; 4061685.11 north; 594444.05 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:45:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868337.80 2024-11-06T04:45:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:39Z- waiting for 1730868341.80, time is 1730868339.01 2024-11-06T04:45:39Z- AHRS_M2 msg: 1730868339.42 epoch sec; seqNo:1428 2024-11-06T04:45:39Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:45:39Z- handleDepth()- Depth_Keller msg: 1730868339.42 epoch sec; seqNo:1540; depth 49.85 2024-11-06T04:45:39Z- handleDVL() - DVL msg epochMillisec: 1730868339.44 2024-11-06T04:45:39Z- handleDvl() - RDI_Pathfinder msg: 1730868339.44 sec; seqNo:554; #beams:4 2024-11-06T04:45:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.38 , 29.67 , 33.03 , 32.72 2024-11-06T04:45:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , -0.03 2024-11-06T04:45:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868339.45 epoch sec; seqNo:1540; 4061685.11 north; 594444.05 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:45:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868337.80 2024-11-06T04:45:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:39Z- waiting for 1730868341.80, time is 1730868339.45 2024-11-06T04:45:39Z- AHRS_M2 msg: 1730868339.80 epoch sec; seqNo:1429 2024-11-06T04:45:39Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:45:39Z- handleDepth()- Depth_Keller msg: 1730868339.81 epoch sec; seqNo:1541; depth 49.86 2024-11-06T04:45:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868339.83 epoch sec; seqNo:1541; 4061685.11 north; 594444.05 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:45:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868337.80 2024-11-06T04:45:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:39Z- waiting for 1730868341.80, time is 1730868339.83 2024-11-06T04:45:40Z- AHRS_M2 msg: 1730868340.21 epoch sec; seqNo:1430 2024-11-06T04:45:40Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:45:40Z- handleDepth()- Depth_Keller msg: 1730868340.22 epoch sec; seqNo:1542; depth 49.87 2024-11-06T04:45:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868340.23 epoch sec; seqNo:1542; 4061685.13 north; 594445.27 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:45:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868337.80 2024-11-06T04:45:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:40Z- waiting for 1730868341.80, time is 1730868340.23 2024-11-06T04:45:40Z- AHRS_M2 msg: 1730868340.61 epoch sec; seqNo:1431 2024-11-06T04:45:40Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:45:40Z- handleDepth()- Depth_Keller msg: 1730868340.61 epoch sec; seqNo:1543; depth 49.89 2024-11-06T04:45:40Z- handleDVL() - DVL msg epochMillisec: 1730868340.64 2024-11-06T04:45:40Z- handleDvl() - RDI_Pathfinder msg: 1730868340.64 sec; seqNo:555; #beams:4 2024-11-06T04:45:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.53 , 29.67 , 32.88 , 33.03 2024-11-06T04:45:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , -0.03 2024-11-06T04:45:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868340.66 epoch sec; seqNo:1543; 4061685.50 north; 594445.27 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:45:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868337.80 2024-11-06T04:45:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:40Z- waiting for 1730868341.80, time is 1730868340.66 2024-11-06T04:45:41Z- AHRS_M2 msg: 1730868341.01 epoch sec; seqNo:1432 2024-11-06T04:45:41Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:45:41Z- handleDepth()- Depth_Keller msg: 1730868341.01 epoch sec; seqNo:1544; depth 49.88 2024-11-06T04:45:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868341.03 epoch sec; seqNo:1544; 4061685.50 north; 594445.27 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T04:45:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868337.80 2024-11-06T04:45:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:41Z- waiting for 1730868341.80, time is 1730868341.03 2024-11-06T04:45:41Z- AHRS_M2 msg: 1730868341.44 epoch sec; seqNo:1433 2024-11-06T04:45:41Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:45:41Z- handleDepth()- Depth_Keller msg: 1730868341.45 epoch sec; seqNo:1545; depth 49.90 2024-11-06T04:45:41Z- handleDVL() - DVL msg epochMillisec: 1730868341.47 2024-11-06T04:45:41Z- handleDvl() - RDI_Pathfinder msg: 1730868341.47 sec; seqNo:556; #beams:4 2024-11-06T04:45:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.23 , 29.67 , 32.88 , 33.03 2024-11-06T04:45:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.08 , -0.02 2024-11-06T04:45:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868341.49 epoch sec; seqNo:1545; 4061685.52 north; 594446.49 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:45:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868337.80 2024-11-06T04:45:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:41Z- waiting for 1730868341.80, time is 1730868341.49 2024-11-06T04:45:41Z- AHRS_M2 msg: 1730868341.82 epoch sec; seqNo:1434 2024-11-06T04:45:41Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:45:41Z- handleDepth()- Depth_Keller msg: 1730868341.83 epoch sec; seqNo:1546; depth 49.90 2024-11-06T04:45:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868341.86 epoch sec; seqNo:1546; 4061685.52 north; 594446.49 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:45:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868337.80 2024-11-06T04:45:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:41Z- waiting for 1730868341.80, time is 1730868341.86 2024-11-06T04:45:41Z- LcmTrn::updateTrn() >>>> pose time 1730868341.86, meas time 1730868341.47 <<<< 2024-11-06T04:45:41Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868341.86 2024-11-06T04:45:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868341.47 sec, ping # 0; waiting for INS data more recent than 1730868337.80... 2024-11-06T04:45:41Z- TerrainNav::motionUpdate() - currEstimate = (4061686, 594446.49, 49.90) 2024-11-06T04:45:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:45:41Z- TerrainNav:: SubcloudNIS is 0.67, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:45:42Z- TNavPF::Projecting Measurements 2024-11-06T04:45:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:45:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:45:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:45:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:45:42Z- TNavFilter::4 good beams. 2024-11-06T04:45:42Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:45:42Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:45:42Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:45:42Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:45:42Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:45:42Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:45:42Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:45:42Z- TerrainMapOctree: (vehN, vehE ) = (4061685.48, 594446.14). 2024-11-06T04:45:42Z- TerrainMapOctree: Min Distance = 206.48. 2024-11-06T04:45:42Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:45:42Z- Forcing Subcloud Comparison 2024-11-06T04:45:42Z- Weighting particles with subcloud comparison 2024-11-06T04:45:42Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:42Z- meanExpectedMeasDiff: -0.474826 delta_rms_squared: 1.439358 alpha: 0.986295 2024-11-06T04:45:42Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:42Z- meanExpectedMeasDiff: 0.238716 delta_rms_squared: 1.567328 alpha: 0.987400 2024-11-06T04:45:42Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:42Z- meanExpectedMeasDiff: -2.569585 delta_rms_squared: 1.202958 alpha: 0.983646 2024-11-06T04:45:42Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:42Z- meanExpectedMeasDiff: 1.071827 delta_rms_squared: 2.025042 alpha: 0.990220 2024-11-06T04:45:42Z- Subcloud NIS: 0.653961 2024-11-06T04:45:42Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:45:42Z- TNavPF:: sumWeights = 0.192042 2024-11-06T04:45:42Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:45:42Z- TNavPF::First Term of Map Covariance Matrix is 0.12 2024-11-06T04:45:42Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.03 2024-11-06T04:45:42Z- TerrainNav::Resampling particle filter... 2024-11-06T04:45:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868341.47 sec. 2024-11-06T04:45:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:45:42Z- variances([0], [2], and psi_berg) = 0.302794 0.441244 0.000000 2024-11-06T04:45:42Z- publishEstimatesDecoder() - at 1730868341.86 sec 2024-11-06T04:45:42Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:45:42Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061629.65 594451.46 55.75 2024-11-06T04:45:42Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061629.16 594450.64 54.72 lat/lon: 36.70 -121.94 2024-11-06T04:45:42Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.30 0.44 0.61 0.00 2024-11-06T04:45:42Z- AHRS_M2 msg: 1730868342.22 epoch sec; seqNo:1435 2024-11-06T04:45:42Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:45:42Z- handleDepth()- Depth_Keller msg: 1730868342.23 epoch sec; seqNo:1547; depth 49.91 2024-11-06T04:45:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868342.24 epoch sec; seqNo:1547; 4061685.52 north; 594446.49 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:45:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868341.86 2024-11-06T04:45:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:42Z- waiting for 1730868345.86, time is 1730868342.24 2024-11-06T04:45:42Z- AHRS_M2 msg: 1730868342.62 epoch sec; seqNo:1436 2024-11-06T04:45:42Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:45:42Z- handleDepth()- Depth_Keller msg: 1730868342.63 epoch sec; seqNo:1548; depth 49.92 2024-11-06T04:45:42Z- handleDVL() - DVL msg epochMillisec: 1730868342.65 2024-11-06T04:45:42Z- handleDvl() - RDI_Pathfinder msg: 1730868342.65 sec; seqNo:557; #beams:4 2024-11-06T04:45:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.23 , 29.67 , 32.88 , 33.03 2024-11-06T04:45:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , -0.02 2024-11-06T04:45:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868342.66 epoch sec; seqNo:1548; 4061685.52 north; 594446.49 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:45:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868341.86 2024-11-06T04:45:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:42Z- waiting for 1730868345.86, time is 1730868342.66 2024-11-06T04:45:43Z- AHRS_M2 msg: 1730868343.03 epoch sec; seqNo:1437 2024-11-06T04:45:43Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:45:43Z- handleDepth()- Depth_Keller msg: 1730868343.03 epoch sec; seqNo:1549; depth 49.93 2024-11-06T04:45:43Z- handleDVL() - DVL msg epochMillisec: 1730868343.03 2024-11-06T04:45:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:45:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:45:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:45:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:45:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:45:43Z- handleDvl() - RDI_Pathfinder msg: 1730868343.03 sec; seqNo:558; #beams:0 2024-11-06T04:45:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:45:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.08 , -0.02 2024-11-06T04:45:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868343.05 epoch sec; seqNo:1549; 4061685.53 north; 594447.71 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T04:45:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868341.86 2024-11-06T04:45:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:43Z- waiting for 1730868345.86, time is 1730868343.05 2024-11-06T04:45:43Z- AHRS_M2 msg: 1730868343.47 epoch sec; seqNo:1438 2024-11-06T04:45:43Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:45:43Z- handleDepth()- Depth_Keller msg: 1730868343.48 epoch sec; seqNo:1550; depth 49.95 2024-11-06T04:45:43Z- handleDVL() - DVL msg epochMillisec: 1730868343.50 2024-11-06T04:45:43Z- handleDvl() - RDI_Pathfinder msg: 1730868343.50 sec; seqNo:559; #beams:4 2024-11-06T04:45:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.38 , 29.67 , 32.88 , 33.18 2024-11-06T04:45:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.06 , -0.01 2024-11-06T04:45:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868343.53 epoch sec; seqNo:1550; 4061685.91 north; 594447.70 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T04:45:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868341.86 2024-11-06T04:45:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:43Z- waiting for 1730868345.86, time is 1730868343.53 2024-11-06T04:45:43Z- AHRS_M2 msg: 1730868343.84 epoch sec; seqNo:1439 2024-11-06T04:45:43Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:45:43Z- handleDepth()- Depth_Keller msg: 1730868343.85 epoch sec; seqNo:1551; depth 49.96 2024-11-06T04:45:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868343.86 epoch sec; seqNo:1551; 4061685.91 north; 594447.70 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:45:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868341.86 2024-11-06T04:45:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:44Z- waiting for 1730868345.86, time is 1730868343.86 2024-11-06T04:45:44Z- AHRS_M2 msg: 1730868344.24 epoch sec; seqNo:1440 2024-11-06T04:45:44Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:45:44Z- handleDepth()- Depth_Keller msg: 1730868344.24 epoch sec; seqNo:1552; depth 49.96 2024-11-06T04:45:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868344.26 epoch sec; seqNo:1552; 4061685.91 north; 594447.70 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:45:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868341.86 2024-11-06T04:45:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:44Z- waiting for 1730868345.86, time is 1730868344.26 2024-11-06T04:45:44Z- AHRS_M2 msg: 1730868344.64 epoch sec; seqNo:1441 2024-11-06T04:45:44Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:45:44Z- handleDepth()- Depth_Keller msg: 1730868344.65 epoch sec; seqNo:1553; depth 49.97 2024-11-06T04:45:44Z- handleDVL() - DVL msg epochMillisec: 1730868344.67 2024-11-06T04:45:44Z- handleDvl() - RDI_Pathfinder msg: 1730868344.67 sec; seqNo:560; #beams:4 2024-11-06T04:45:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.08 , 29.51 , 32.57 , 33.03 2024-11-06T04:45:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.05 , -0.01 2024-11-06T04:45:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868344.69 epoch sec; seqNo:1553; 4061685.92 north; 594448.93 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T04:45:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868341.86 2024-11-06T04:45:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:44Z- waiting for 1730868345.86, time is 1730868344.69 2024-11-06T04:45:45Z- AHRS_M2 msg: 1730868345.05 epoch sec; seqNo:1442 2024-11-06T04:45:45Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:45:45Z- handleDepth()- Depth_Keller msg: 1730868345.05 epoch sec; seqNo:1554; depth 49.97 2024-11-06T04:45:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868345.07 epoch sec; seqNo:1554; 4061685.92 north; 594448.93 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T04:45:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868341.86 2024-11-06T04:45:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:45Z- waiting for 1730868345.86, time is 1730868345.07 2024-11-06T04:45:45Z- AHRS_M2 msg: 1730868345.49 epoch sec; seqNo:1443 2024-11-06T04:45:45Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:45:45Z- handleDepth()- Depth_Keller msg: 1730868345.51 epoch sec; seqNo:1555; depth 49.99 2024-11-06T04:45:45Z- handleDVL() - DVL msg epochMillisec: 1730868345.54 2024-11-06T04:45:45Z- handleDvl() - RDI_Pathfinder msg: 1730868345.54 sec; seqNo:561; #beams:4 2024-11-06T04:45:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.92 , 29.51 , 32.26 , 32.88 2024-11-06T04:45:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.01 2024-11-06T04:45:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868345.58 epoch sec; seqNo:1555; 4061685.92 north; 594448.93 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T04:45:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868341.86 2024-11-06T04:45:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:45Z- waiting for 1730868345.86, time is 1730868345.58 2024-11-06T04:45:45Z- handleDepth()- Depth_Keller msg: 1730868345.87 epoch sec; seqNo:1556; depth 50.00 2024-11-06T04:45:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868345.88 epoch sec; seqNo:1556; 4061685.94 north; 594450.15 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T04:45:46Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:45:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868341.86 2024-11-06T04:45:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:46Z- waiting for 1730868345.86, time is 1730868345.88 2024-11-06T04:45:46Z- LcmTrn::updateTrn() >>>> pose time 1730868345.88, meas time 1730868345.54 <<<< 2024-11-06T04:45:46Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868345.88 2024-11-06T04:45:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868345.54 sec, ping # 0; waiting for INS data more recent than 1730868341.86... 2024-11-06T04:45:46Z- TerrainNav::motionUpdate() - currEstimate = (4061686, 594450.15, 50.00) 2024-11-06T04:45:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:45:46Z- TerrainNav:: SubcloudNIS is 0.65, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:45:46Z- TNavPF::Projecting Measurements 2024-11-06T04:45:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:45:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:45:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:45:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:45:46Z- TNavFilter::4 good beams. 2024-11-06T04:45:46Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:45:46Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:45:46Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:45:46Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:45:46Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:45:46Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:45:46Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:45:46Z- TerrainMapOctree: (vehN, vehE ) = (4061685.90, 594449.83). 2024-11-06T04:45:46Z- TerrainMapOctree: Min Distance = 209.86. 2024-11-06T04:45:46Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:45:46Z- Forcing Subcloud Comparison 2024-11-06T04:45:46Z- Weighting particles with subcloud comparison 2024-11-06T04:45:46Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:46Z- meanExpectedMeasDiff: -1.010168 delta_rms_squared: 0.939516 alpha: 0.979156 2024-11-06T04:45:46Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:46Z- meanExpectedMeasDiff: -1.106562 delta_rms_squared: 1.315216 alpha: 0.985021 2024-11-06T04:45:46Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:46Z- meanExpectedMeasDiff: -3.988129 delta_rms_squared: 1.263111 alpha: 0.984413 2024-11-06T04:45:46Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:46Z- meanExpectedMeasDiff: 1.673040 delta_rms_squared: 1.006335 alpha: 0.980513 2024-11-06T04:45:46Z- Subcloud NIS: 0.668416 2024-11-06T04:45:46Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:45:46Z- TNavPF:: sumWeights = 0.051424 2024-11-06T04:45:46Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:45:46Z- TNavPF::First Term of Map Covariance Matrix is 0.03 2024-11-06T04:45:46Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T04:45:46Z- TerrainNav::Resampling particle filter... 2024-11-06T04:45:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868345.54 sec. 2024-11-06T04:45:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:45:46Z- variances([0], [2], and psi_berg) = 0.295168 0.527147 0.000000 2024-11-06T04:45:46Z- publishEstimatesDecoder() - at 1730868345.88 sec 2024-11-06T04:45:46Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:45:46Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061629.95 594455.11 55.29 2024-11-06T04:45:46Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061629.62 594454.38 55.48 lat/lon: 36.70 -121.94 2024-11-06T04:45:46Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.30 0.53 0.71 0.00 2024-11-06T04:45:46Z- AHRS_M2 msg: 1730868346.26 epoch sec; seqNo:1444 2024-11-06T04:45:46Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:45:46Z- handleDepth()- Depth_Keller msg: 1730868346.27 epoch sec; seqNo:1557; depth 50.01 2024-11-06T04:45:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868346.28 epoch sec; seqNo:1557; 4061685.94 north; 594450.15 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:45:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868345.88 2024-11-06T04:45:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:46Z- waiting for 1730868349.88, time is 1730868346.28 2024-11-06T04:45:46Z- AHRS_M2 msg: 1730868346.66 epoch sec; seqNo:1445 2024-11-06T04:45:46Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:45:46Z- handleDepth()- Depth_Keller msg: 1730868346.67 epoch sec; seqNo:1558; depth 50.01 2024-11-06T04:45:46Z- handleDVL() - DVL msg epochMillisec: 1730868346.70 2024-11-06T04:45:46Z- handleDvl() - RDI_Pathfinder msg: 1730868346.70 sec; seqNo:562; #beams:4 2024-11-06T04:45:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.92 , 29.21 , 31.96 , 33.03 2024-11-06T04:45:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , -0.01 2024-11-06T04:45:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868346.72 epoch sec; seqNo:1558; 4061685.94 north; 594450.15 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:45:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868345.88 2024-11-06T04:45:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:46Z- waiting for 1730868349.88, time is 1730868346.72 2024-11-06T04:45:47Z- AHRS_M2 msg: 1730868347.07 epoch sec; seqNo:1446 2024-11-06T04:45:47Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:45:47Z- handleDepth()- Depth_Keller msg: 1730868347.07 epoch sec; seqNo:1559; depth 50.02 2024-11-06T04:45:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868347.09 epoch sec; seqNo:1559; 4061685.94 north; 594450.15 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:45:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868345.88 2024-11-06T04:45:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:47Z- waiting for 1730868349.88, time is 1730868347.09 2024-11-06T04:45:47Z- AHRS_M2 msg: 1730868347.50 epoch sec; seqNo:1447 2024-11-06T04:45:47Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:45:47Z- handleDepth()- Depth_Keller msg: 1730868347.52 epoch sec; seqNo:1560; depth 50.02 2024-11-06T04:45:47Z- handleDVL() - DVL msg epochMillisec: 1730868347.55 2024-11-06T04:45:47Z- handleDvl() - RDI_Pathfinder msg: 1730868347.55 sec; seqNo:563; #beams:4 2024-11-06T04:45:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.92 , 28.90 , 31.96 , 32.72 2024-11-06T04:45:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , -0.02 2024-11-06T04:45:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868347.57 epoch sec; seqNo:1560; 4061685.95 north; 594451.37 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:45:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868345.88 2024-11-06T04:45:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:47Z- waiting for 1730868349.88, time is 1730868347.57 2024-11-06T04:45:47Z- AHRS_M2 msg: 1730868347.88 epoch sec; seqNo:1448 2024-11-06T04:45:47Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:45:47Z- handleDepth()- Depth_Keller msg: 1730868347.89 epoch sec; seqNo:1561; depth 50.05 2024-11-06T04:45:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868347.91 epoch sec; seqNo:1561; 4061686.33 north; 594451.36 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T04:45:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868345.88 2024-11-06T04:45:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:48Z- waiting for 1730868349.88, time is 1730868347.91 2024-11-06T04:45:48Z- AHRS_M2 msg: 1730868348.28 epoch sec; seqNo:1449 2024-11-06T04:45:48Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:45:48Z- handleDepth()- Depth_Keller msg: 1730868348.28 epoch sec; seqNo:1562; depth 50.06 2024-11-06T04:45:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868348.29 epoch sec; seqNo:1562; 4061686.33 north; 594451.36 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T04:45:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868345.88 2024-11-06T04:45:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:48Z- waiting for 1730868349.88, time is 1730868348.29 2024-11-06T04:45:48Z- AHRS_M2 msg: 1730868348.69 epoch sec; seqNo:1450 2024-11-06T04:45:48Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:45:48Z- handleDepth()- Depth_Keller msg: 1730868348.69 epoch sec; seqNo:1563; depth 50.07 2024-11-06T04:45:48Z- handleDVL() - DVL msg epochMillisec: 1730868348.71 2024-11-06T04:45:48Z- handleDvl() - RDI_Pathfinder msg: 1730868348.71 sec; seqNo:564; #beams:4 2024-11-06T04:45:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.62 , 29.05 , 31.81 , 32.26 2024-11-06T04:45:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , -0.02 2024-11-06T04:45:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868348.73 epoch sec; seqNo:1563; 4061686.33 north; 594451.36 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T04:45:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868345.88 2024-11-06T04:45:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:48Z- waiting for 1730868349.88, time is 1730868348.73 2024-11-06T04:45:49Z- AHRS_M2 msg: 1730868349.09 epoch sec; seqNo:1451 2024-11-06T04:45:49Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:45:49Z- handleDepth()- Depth_Keller msg: 1730868349.10 epoch sec; seqNo:1564; depth 50.07 2024-11-06T04:45:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868349.11 epoch sec; seqNo:1564; 4061686.34 north; 594452.58 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T04:45:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868345.88 2024-11-06T04:45:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:49Z- waiting for 1730868349.88, time is 1730868349.11 2024-11-06T04:45:49Z- AHRS_M2 msg: 1730868349.49 epoch sec; seqNo:1452 2024-11-06T04:45:49Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:45:49Z- handleDepth()- Depth_Keller msg: 1730868349.53 epoch sec; seqNo:1565; depth 50.09 2024-11-06T04:45:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868349.57 epoch sec; seqNo:1565; 4061686.34 north; 594452.58 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T04:45:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868345.88 2024-11-06T04:45:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:49Z- waiting for 1730868349.88, time is 1730868349.57 2024-11-06T04:45:49Z- AHRS_M2 msg: 1730868349.90 epoch sec; seqNo:1453 2024-11-06T04:45:49Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:45:49Z- handleDepth()- Depth_Keller msg: 1730868349.91 epoch sec; seqNo:1566; depth 50.13 2024-11-06T04:45:49Z- handleDVL() - DVL msg epochMillisec: 1730868349.95 2024-11-06T04:45:49Z- handleDvl() - RDI_Pathfinder msg: 1730868349.95 sec; seqNo:565; #beams:4 2024-11-06T04:45:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.77 , 29.36 , 31.65 , 32.11 2024-11-06T04:45:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , -0.01 2024-11-06T04:45:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868349.96 epoch sec; seqNo:1566; 4061686.34 north; 594452.58 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T04:45:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868345.88 2024-11-06T04:45:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:50Z- waiting for 1730868349.88, time is 1730868349.96 2024-11-06T04:45:50Z- LcmTrn::updateTrn() >>>> pose time 1730868349.96, meas time 1730868349.95 <<<< 2024-11-06T04:45:50Z- LcmTrn::updateTrn() equating pose 1730868349.95 and meas times 1730868349.95 2024-11-06T04:45:50Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868349.95 2024-11-06T04:45:50Z- TerrainNav::motionUpdate() - currEstimate = (4061686, 594452.58, 50.13) 2024-11-06T04:45:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:45:50Z- TerrainNav:: SubcloudNIS is 0.67, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:45:50Z- TNavPF::Projecting Measurements 2024-11-06T04:45:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:45:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:45:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:45:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:45:50Z- TNavFilter::4 good beams. 2024-11-06T04:45:50Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:45:50Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:45:50Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:45:50Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:45:50Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:45:50Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:45:50Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:45:50Z- TerrainMapOctree: (vehN, vehE ) = (4061686.34, 594452.58). 2024-11-06T04:45:50Z- TerrainMapOctree: Min Distance = 212.35. 2024-11-06T04:45:50Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:45:50Z- Forcing Subcloud Comparison 2024-11-06T04:45:50Z- Weighting particles with subcloud comparison 2024-11-06T04:45:50Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:50Z- meanExpectedMeasDiff: -0.349859 delta_rms_squared: 1.065827 alpha: 0.981581 2024-11-06T04:45:50Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:50Z- meanExpectedMeasDiff: -1.126525 delta_rms_squared: 1.315085 alpha: 0.985020 2024-11-06T04:45:50Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:50Z- meanExpectedMeasDiff: -3.861933 delta_rms_squared: 1.245304 alpha: 0.984194 2024-11-06T04:45:50Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:50Z- meanExpectedMeasDiff: 0.949172 delta_rms_squared: 1.322642 alpha: 0.985104 2024-11-06T04:45:50Z- Subcloud NIS: 0.661360 2024-11-06T04:45:50Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:45:50Z- TNavPF:: sumWeights = 0.075047 2024-11-06T04:45:50Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:45:50Z- TNavPF::First Term of Map Covariance Matrix is 0.04 2024-11-06T04:45:50Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T04:45:50Z- TerrainNav::Resampling particle filter... 2024-11-06T04:45:50Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868349.95 sec, ping # 0. 2024-11-06T04:45:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:45:50Z- variances([0], [2], and psi_berg) = 0.284684 0.489735 0.000000 2024-11-06T04:45:50Z- publishEstimatesDecoder() - at 1730868349.95 sec 2024-11-06T04:45:50Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:45:50Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061630.49 594456.14 55.95 2024-11-06T04:45:50Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061630.02 594456.85 56.19 lat/lon: 36.70 -121.94 2024-11-06T04:45:50Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.28 0.49 0.42 0.00 2024-11-06T04:45:50Z- AHRS_M2 msg: 1730868350.30 epoch sec; seqNo:1454 2024-11-06T04:45:50Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:45:50Z- handleDepth()- Depth_Keller msg: 1730868350.31 epoch sec; seqNo:1567; depth 50.14 2024-11-06T04:45:50Z- handleDVL() - DVL msg epochMillisec: 1730868350.32 2024-11-06T04:45:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:45:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:45:50Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:45:50Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:45:50Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:45:50Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:45:50Z- handleDvl() - RDI_Pathfinder msg: 1730868350.32 sec; seqNo:566; #beams:0 2024-11-06T04:45:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:45:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , -0.01 2024-11-06T04:45:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868350.34 epoch sec; seqNo:1567; 4061686.36 north; 594453.80 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:45:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868349.95 2024-11-06T04:45:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:50Z- waiting for 1730868353.95, time is 1730868350.34 2024-11-06T04:45:50Z- AHRS_M2 msg: 1730868350.70 epoch sec; seqNo:1455 2024-11-06T04:45:50Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:45:50Z- handleDepth()- Depth_Keller msg: 1730868350.71 epoch sec; seqNo:1568; depth 50.15 2024-11-06T04:45:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868350.74 epoch sec; seqNo:1568; 4061686.36 north; 594453.80 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T04:45:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868349.95 2024-11-06T04:45:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:50Z- waiting for 1730868353.95, time is 1730868350.74 2024-11-06T04:45:51Z- AHRS_M2 msg: 1730868351.11 epoch sec; seqNo:1456 2024-11-06T04:45:51Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:45:51Z- handleDepth()- Depth_Keller msg: 1730868351.11 epoch sec; seqNo:1569; depth 50.17 2024-11-06T04:45:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868351.13 epoch sec; seqNo:1569; 4061686.36 north; 594453.80 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T04:45:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868349.95 2024-11-06T04:45:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:51Z- waiting for 1730868353.95, time is 1730868351.13 2024-11-06T04:45:51Z- AHRS_M2 msg: 1730868351.52 epoch sec; seqNo:1457 2024-11-06T04:45:51Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:45:51Z- handleDepth()- Depth_Keller msg: 1730868351.52 epoch sec; seqNo:1570; depth 50.19 2024-11-06T04:45:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868351.56 epoch sec; seqNo:1570; 4061686.36 north; 594453.80 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:45:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868349.95 2024-11-06T04:45:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:51Z- waiting for 1730868353.95, time is 1730868351.56 2024-11-06T04:45:51Z- AHRS_M2 msg: 1730868351.92 epoch sec; seqNo:1458 2024-11-06T04:45:51Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:45:51Z- handleDepth()- Depth_Keller msg: 1730868351.92 epoch sec; seqNo:1571; depth 50.20 2024-11-06T04:45:51Z- handleDVL() - DVL msg epochMillisec: 1730868351.95 2024-11-06T04:45:51Z- handleDvl() - RDI_Pathfinder msg: 1730868351.95 sec; seqNo:567; #beams:4 2024-11-06T04:45:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.77 , 29.05 , 31.35 , 32.42 2024-11-06T04:45:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.07 , 0.02 2024-11-06T04:45:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868351.97 epoch sec; seqNo:1571; 4061686.75 north; 594455.02 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:45:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868349.95 2024-11-06T04:45:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:52Z- waiting for 1730868353.95, time is 1730868351.97 2024-11-06T04:45:52Z- AHRS_M2 msg: 1730868352.32 epoch sec; seqNo:1459 2024-11-06T04:45:52Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:45:52Z- handleDepth()- Depth_Keller msg: 1730868352.33 epoch sec; seqNo:1572; depth 50.22 2024-11-06T04:45:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868352.34 epoch sec; seqNo:1572; 4061686.75 north; 594455.02 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T04:45:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868349.95 2024-11-06T04:45:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:52Z- waiting for 1730868353.95, time is 1730868352.34 2024-11-06T04:45:52Z- AHRS_M2 msg: 1730868352.73 epoch sec; seqNo:1460 2024-11-06T04:45:52Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:45:52Z- handleDepth()- Depth_Keller msg: 1730868352.73 epoch sec; seqNo:1573; depth 50.23 2024-11-06T04:45:52Z- handleDVL() - DVL msg epochMillisec: 1730868352.75 2024-11-06T04:45:52Z- handleDvl() - RDI_Pathfinder msg: 1730868352.75 sec; seqNo:568; #beams:4 2024-11-06T04:45:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.77 , 29.36 , 31.19 , 32.88 2024-11-06T04:45:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , 0.03 2024-11-06T04:45:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868352.78 epoch sec; seqNo:1573; 4061686.75 north; 594455.02 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T04:45:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868349.95 2024-11-06T04:45:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:52Z- waiting for 1730868353.95, time is 1730868352.78 2024-11-06T04:45:53Z- AHRS_M2 msg: 1730868353.13 epoch sec; seqNo:1461 2024-11-06T04:45:53Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:45:53Z- handleDepth()- Depth_Keller msg: 1730868353.13 epoch sec; seqNo:1574; depth 50.25 2024-11-06T04:45:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868353.15 epoch sec; seqNo:1574; 4061686.75 north; 594455.02 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T04:45:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868349.95 2024-11-06T04:45:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:53Z- waiting for 1730868353.95, time is 1730868353.15 2024-11-06T04:45:53Z- AHRS_M2 msg: 1730868353.53 epoch sec; seqNo:1462 2024-11-06T04:45:53Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:45:53Z- handleDepth()- Depth_Keller msg: 1730868353.54 epoch sec; seqNo:1575; depth 50.26 2024-11-06T04:45:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868353.57 epoch sec; seqNo:1575; 4061686.76 north; 594456.24 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:45:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868349.95 2024-11-06T04:45:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:53Z- waiting for 1730868353.95, time is 1730868353.57 2024-11-06T04:45:53Z- AHRS_M2 msg: 1730868353.94 epoch sec; seqNo:1463 2024-11-06T04:45:53Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:45:53Z- handleDepth()- Depth_Keller msg: 1730868353.95 epoch sec; seqNo:1576; depth 50.27 2024-11-06T04:45:54Z- handleDVL() - DVL msg epochMillisec: 1730868353.99 2024-11-06T04:45:54Z- handleDvl() - RDI_Pathfinder msg: 1730868353.99 sec; seqNo:569; #beams:4 2024-11-06T04:45:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.23 , 29.36 , 31.19 , 32.88 2024-11-06T04:45:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , 0.04 2024-11-06T04:45:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868354.01 epoch sec; seqNo:1576; 4061686.76 north; 594456.24 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T04:45:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868349.95 2024-11-06T04:45:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:54Z- waiting for 1730868353.95, time is 1730868354.01 2024-11-06T04:45:54Z- LcmTrn::updateTrn() >>>> pose time 1730868354.01, meas time 1730868353.99 <<<< 2024-11-06T04:45:54Z- LcmTrn::updateTrn() equating pose 1730868353.99 and meas times 1730868353.99 2024-11-06T04:45:54Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868353.99 2024-11-06T04:45:54Z- TerrainNav::motionUpdate() - currEstimate = (4061687, 594456.24, 50.27) 2024-11-06T04:45:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:45:54Z- TerrainNav:: SubcloudNIS is 0.66, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:45:54Z- TNavPF::Projecting Measurements 2024-11-06T04:45:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:45:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:45:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:45:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:45:54Z- TNavFilter::4 good beams. 2024-11-06T04:45:54Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:45:54Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:45:54Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:45:54Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:45:54Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:45:54Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:45:54Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:45:54Z- TerrainMapOctree: (vehN, vehE ) = (4061686.76, 594456.24). 2024-11-06T04:45:54Z- TerrainMapOctree: Min Distance = 215.72. 2024-11-06T04:45:54Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:45:54Z- Forcing Subcloud Comparison 2024-11-06T04:45:54Z- Weighting particles with subcloud comparison 2024-11-06T04:45:54Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:54Z- meanExpectedMeasDiff: 0.606265 delta_rms_squared: 1.113914 alpha: 0.982362 2024-11-06T04:45:54Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:54Z- meanExpectedMeasDiff: -1.147169 delta_rms_squared: 0.829470 alpha: 0.976456 2024-11-06T04:45:54Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:54Z- meanExpectedMeasDiff: -3.525517 delta_rms_squared: 0.885188 alpha: 0.977905 2024-11-06T04:45:54Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:54Z- meanExpectedMeasDiff: 1.650043 delta_rms_squared: 0.882103 alpha: 0.977830 2024-11-06T04:45:54Z- Subcloud NIS: 0.655491 2024-11-06T04:45:54Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:45:54Z- TNavPF:: sumWeights = 0.068495 2024-11-06T04:45:54Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:45:54Z- TNavPF::First Term of Map Covariance Matrix is 0.10 2024-11-06T04:45:54Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T04:45:54Z- TerrainNav::Resampling particle filter... 2024-11-06T04:45:54Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868353.99 sec, ping # 0. 2024-11-06T04:45:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:45:54Z- variances([0], [2], and psi_berg) = 0.255330 0.420619 0.000000 2024-11-06T04:45:54Z- publishEstimatesDecoder() - at 1730868353.99 sec 2024-11-06T04:45:54Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:45:54Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061630.80 594459.83 56.02 2024-11-06T04:45:54Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061630.48 594460.60 56.65 lat/lon: 36.70 -121.94 2024-11-06T04:45:54Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.42 0.34 0.00 2024-11-06T04:45:54Z- AHRS_M2 msg: 1730868354.34 epoch sec; seqNo:1464 2024-11-06T04:45:54Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:45:54Z- handleDepth()- Depth_Keller msg: 1730868354.34 epoch sec; seqNo:1577; depth 50.29 2024-11-06T04:45:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868354.37 epoch sec; seqNo:1577; 4061686.76 north; 594456.24 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:45:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868353.99 2024-11-06T04:45:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:54Z- waiting for 1730868357.99, time is 1730868354.37 2024-11-06T04:45:54Z- AHRS_M2 msg: 1730868354.76 epoch sec; seqNo:1465 2024-11-06T04:45:54Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.53 psi 2024-11-06T04:45:54Z- handleDepth()- Depth_Keller msg: 1730868354.76 epoch sec; seqNo:1578; depth 50.30 2024-11-06T04:45:54Z- handleDVL() - DVL msg epochMillisec: 1730868354.79 2024-11-06T04:45:54Z- handleDvl() - RDI_Pathfinder msg: 1730868354.79 sec; seqNo:570; #beams:4 2024-11-06T04:45:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.77 , 29.51 , 31.04 , 33.18 2024-11-06T04:45:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , 0.04 2024-11-06T04:45:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868354.80 epoch sec; seqNo:1578; 4061686.78 north; 594457.46 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:45:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868353.99 2024-11-06T04:45:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:54Z- waiting for 1730868357.99, time is 1730868354.80 2024-11-06T04:45:55Z- AHRS_M2 msg: 1730868355.16 epoch sec; seqNo:1466 2024-11-06T04:45:55Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.53 psi 2024-11-06T04:45:55Z- handleDepth()- Depth_Keller msg: 1730868355.16 epoch sec; seqNo:1579; depth 50.30 2024-11-06T04:45:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868355.18 epoch sec; seqNo:1579; 4061686.78 north; 594457.46 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:45:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868353.99 2024-11-06T04:45:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:55Z- waiting for 1730868357.99, time is 1730868355.18 2024-11-06T04:45:55Z- AHRS_M2 msg: 1730868355.61 epoch sec; seqNo:1467 2024-11-06T04:45:55Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.53 psi 2024-11-06T04:45:55Z- handleDepth()- Depth_Keller msg: 1730868355.64 epoch sec; seqNo:1580; depth 50.31 2024-11-06T04:45:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868355.67 epoch sec; seqNo:1580; 4061686.78 north; 594457.46 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:45:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868353.99 2024-11-06T04:45:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:55Z- waiting for 1730868357.99, time is 1730868355.67 2024-11-06T04:45:55Z- AHRS_M2 msg: 1730868355.98 epoch sec; seqNo:1468 2024-11-06T04:45:55Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.54 psi 2024-11-06T04:45:55Z- handleDepth()- Depth_Keller msg: 1730868355.98 epoch sec; seqNo:1581; depth 50.33 2024-11-06T04:45:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868356.00 epoch sec; seqNo:1581; 4061686.79 north; 594458.68 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T04:45:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868353.99 2024-11-06T04:45:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:56Z- waiting for 1730868357.99, time is 1730868356.00 2024-11-06T04:45:56Z- AHRS_M2 msg: 1730868356.37 epoch sec; seqNo:1469 2024-11-06T04:45:56Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.54 psi 2024-11-06T04:45:56Z- handleDepth()- Depth_Keller msg: 1730868356.38 epoch sec; seqNo:1582; depth 50.34 2024-11-06T04:45:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868356.38 epoch sec; seqNo:1582; 4061687.17 north; 594458.68 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:45:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868353.99 2024-11-06T04:45:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:56Z- waiting for 1730868357.99, time is 1730868356.38 2024-11-06T04:45:56Z- AHRS_M2 msg: 1730868356.78 epoch sec; seqNo:1470 2024-11-06T04:45:56Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.54 psi 2024-11-06T04:45:56Z- handleDepth()- Depth_Keller msg: 1730868356.82 epoch sec; seqNo:1583; depth 50.34 2024-11-06T04:45:56Z- handleDVL() - DVL msg epochMillisec: 1730868356.88 2024-11-06T04:45:56Z- handleDvl() - RDI_Pathfinder msg: 1730868356.88 sec; seqNo:571; #beams:4 2024-11-06T04:45:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.62 , 29.36 , 31.35 , 32.88 2024-11-06T04:45:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , -0.00 2024-11-06T04:45:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868356.90 epoch sec; seqNo:1583; 4061687.17 north; 594458.68 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:45:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868353.99 2024-11-06T04:45:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:56Z- waiting for 1730868357.99, time is 1730868356.90 2024-11-06T04:45:57Z- AHRS_M2 msg: 1730868357.18 epoch sec; seqNo:1471 2024-11-06T04:45:57Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.54 psi 2024-11-06T04:45:57Z- handleDepth()- Depth_Keller msg: 1730868357.19 epoch sec; seqNo:1584; depth 50.35 2024-11-06T04:45:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868357.21 epoch sec; seqNo:1584; 4061687.17 north; 594458.68 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:45:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868353.99 2024-11-06T04:45:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:57Z- waiting for 1730868357.99, time is 1730868357.21 2024-11-06T04:45:57Z- AHRS_M2 msg: 1730868357.58 epoch sec; seqNo:1472 2024-11-06T04:45:57Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.54 psi 2024-11-06T04:45:57Z- handleDepth()- Depth_Keller msg: 1730868357.59 epoch sec; seqNo:1585; depth 50.34 2024-11-06T04:45:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868357.60 epoch sec; seqNo:1585; 4061687.18 north; 594459.90 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:45:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868353.99 2024-11-06T04:45:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:57Z- waiting for 1730868357.99, time is 1730868357.60 2024-11-06T04:45:58Z- AHRS_M2 msg: 1730868358.01 epoch sec; seqNo:1473 2024-11-06T04:45:58Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.54 psi 2024-11-06T04:45:58Z- handleDepth()- Depth_Keller msg: 1730868358.02 epoch sec; seqNo:1586; depth 50.35 2024-11-06T04:45:58Z- handleDVL() - DVL msg epochMillisec: 1730868358.03 2024-11-06T04:45:58Z- handleDvl() - RDI_Pathfinder msg: 1730868358.03 sec; seqNo:572; #beams:4 2024-11-06T04:45:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.46 , 29.51 , 31.19 , 32.72 2024-11-06T04:45:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , -0.02 2024-11-06T04:45:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868358.06 epoch sec; seqNo:1586; 4061687.18 north; 594459.90 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:45:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868353.99 2024-11-06T04:45:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:58Z- waiting for 1730868357.99, time is 1730868358.06 2024-11-06T04:45:58Z- LcmTrn::updateTrn() >>>> pose time 1730868358.06, meas time 1730868358.03 <<<< 2024-11-06T04:45:58Z- LcmTrn::updateTrn() equating pose 1730868358.03 and meas times 1730868358.03 2024-11-06T04:45:58Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868358.03 2024-11-06T04:45:58Z- TerrainNav::motionUpdate() - currEstimate = (4061687, 594459.90, 50.35) 2024-11-06T04:45:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:45:58Z- TerrainNav:: SubcloudNIS is 0.66, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:45:58Z- TNavPF::Projecting Measurements 2024-11-06T04:45:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:45:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:45:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:45:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:45:58Z- TNavFilter::4 good beams. 2024-11-06T04:45:58Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:45:58Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:45:58Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:45:58Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:45:58Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:45:58Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:45:58Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:45:58Z- TerrainMapOctree: (vehN, vehE ) = (4061687.18, 594459.90). 2024-11-06T04:45:58Z- TerrainMapOctree: Min Distance = 219.09. 2024-11-06T04:45:58Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:45:58Z- Forcing Subcloud Comparison 2024-11-06T04:45:58Z- Weighting particles with subcloud comparison 2024-11-06T04:45:58Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:58Z- meanExpectedMeasDiff: 0.221545 delta_rms_squared: 0.959439 alpha: 0.979580 2024-11-06T04:45:58Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:58Z- meanExpectedMeasDiff: -1.553594 delta_rms_squared: 0.688836 alpha: 0.971785 2024-11-06T04:45:58Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:58Z- meanExpectedMeasDiff: -3.241752 delta_rms_squared: 0.764479 alpha: 0.974505 2024-11-06T04:45:58Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:45:58Z- meanExpectedMeasDiff: 1.815429 delta_rms_squared: 0.820924 alpha: 0.976217 2024-11-06T04:45:58Z- Subcloud NIS: 0.658004 2024-11-06T04:45:58Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:45:58Z- TNavPF:: sumWeights = 0.074839 2024-11-06T04:45:58Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:45:58Z- TNavPF::First Term of Map Covariance Matrix is 0.06 2024-11-06T04:45:58Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T04:45:58Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868358.03 sec, ping # 0. 2024-11-06T04:45:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:45:58Z- variances([0], [2], and psi_berg) = 0.252526 0.410958 0.000000 2024-11-06T04:45:58Z- publishEstimatesDecoder() - at 1730868358.03 sec 2024-11-06T04:45:58Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:45:58Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061632.15 594463.42 57.07 2024-11-06T04:45:58Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061630.90 594464.28 57.00 lat/lon: 36.70 -121.94 2024-11-06T04:45:58Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.41 0.30 0.00 2024-11-06T04:45:58Z- AHRS_M2 msg: 1730868358.39 epoch sec; seqNo:1474 2024-11-06T04:45:58Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.54 psi 2024-11-06T04:45:58Z- handleDepth()- Depth_Keller msg: 1730868358.42 epoch sec; seqNo:1587; depth 50.35 2024-11-06T04:45:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868358.43 epoch sec; seqNo:1587; 4061687.18 north; 594459.90 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:45:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868358.03 2024-11-06T04:45:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:58Z- waiting for 1730868362.03, time is 1730868358.43 2024-11-06T04:45:58Z- AHRS_M2 msg: 1730868358.80 epoch sec; seqNo:1475 2024-11-06T04:45:58Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.55 psi 2024-11-06T04:45:58Z- handleDepth()- Depth_Keller msg: 1730868358.80 epoch sec; seqNo:1588; depth 50.34 2024-11-06T04:45:58Z- handleDVL() - DVL msg epochMillisec: 1730868358.84 2024-11-06T04:45:58Z- handleDvl() - RDI_Pathfinder msg: 1730868358.84 sec; seqNo:573; #beams:4 2024-11-06T04:45:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.62 , 29.36 , 31.35 , 32.57 2024-11-06T04:45:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.04 2024-11-06T04:45:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868358.85 epoch sec; seqNo:1588; 4061687.19 north; 594461.12 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:45:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:45:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868358.03 2024-11-06T04:45:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:58Z- waiting for 1730868362.03, time is 1730868358.85 2024-11-06T04:45:59Z- AHRS_M2 msg: 1730868359.20 epoch sec; seqNo:1476 2024-11-06T04:45:59Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.55 psi 2024-11-06T04:45:59Z- handleDepth()- Depth_Keller msg: 1730868359.20 epoch sec; seqNo:1589; depth 50.34 2024-11-06T04:45:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868359.22 epoch sec; seqNo:1589; 4061687.19 north; 594461.12 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:45:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868358.03 2024-11-06T04:45:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:59Z- waiting for 1730868362.03, time is 1730868359.22 2024-11-06T04:45:59Z- AHRS_M2 msg: 1730868359.60 epoch sec; seqNo:1477 2024-11-06T04:45:59Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.55 psi 2024-11-06T04:45:59Z- handleDepth()- Depth_Keller msg: 1730868359.61 epoch sec; seqNo:1590; depth 50.34 2024-11-06T04:45:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868359.62 epoch sec; seqNo:1590; 4061687.19 north; 594461.12 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:45:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:45:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868358.03 2024-11-06T04:45:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:45:59Z- waiting for 1730868362.03, time is 1730868359.62 2024-11-06T04:46:00Z- AHRS_M2 msg: 1730868360.01 epoch sec; seqNo:1478 2024-11-06T04:46:00Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.55 psi 2024-11-06T04:46:00Z- handleDepth()- Depth_Keller msg: 1730868360.02 epoch sec; seqNo:1591; depth 50.34 2024-11-06T04:46:00Z- handleDVL() - DVL msg epochMillisec: 1730868360.07 2024-11-06T04:46:00Z- handleDvl() - RDI_Pathfinder msg: 1730868360.07 sec; seqNo:574; #beams:4 2024-11-06T04:46:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.46 , 29.21 , 31.65 , 32.42 2024-11-06T04:46:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , -0.04 2024-11-06T04:46:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868360.08 epoch sec; seqNo:1591; 4061687.21 north; 594462.34 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:46:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868358.03 2024-11-06T04:46:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:00Z- waiting for 1730868362.03, time is 1730868360.08 2024-11-06T04:46:00Z- AHRS_M2 msg: 1730868360.42 epoch sec; seqNo:1479 2024-11-06T04:46:00Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.55 psi 2024-11-06T04:46:00Z- handleDepth()- Depth_Keller msg: 1730868360.42 epoch sec; seqNo:1592; depth 50.32 2024-11-06T04:46:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868360.44 epoch sec; seqNo:1592; 4061687.21 north; 594462.34 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:46:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868358.03 2024-11-06T04:46:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:00Z- waiting for 1730868362.03, time is 1730868360.44 2024-11-06T04:46:00Z- AHRS_M2 msg: 1730868360.82 epoch sec; seqNo:1480 2024-11-06T04:46:00Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.55 psi 2024-11-06T04:46:00Z- handleDepth()- Depth_Keller msg: 1730868360.84 epoch sec; seqNo:1593; depth 50.31 2024-11-06T04:46:00Z- handleDVL() - DVL msg epochMillisec: 1730868360.86 2024-11-06T04:46:00Z- handleDvl() - RDI_Pathfinder msg: 1730868360.86 sec; seqNo:575; #beams:4 2024-11-06T04:46:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.16 , 29.36 , 31.50 , 32.42 2024-11-06T04:46:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , -0.05 2024-11-06T04:46:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868360.89 epoch sec; seqNo:1593; 4061687.21 north; 594462.34 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:46:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868358.03 2024-11-06T04:46:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:00Z- waiting for 1730868362.03, time is 1730868360.89 2024-11-06T04:46:01Z- AHRS_M2 msg: 1730868361.22 epoch sec; seqNo:1481 2024-11-06T04:46:01Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.55 psi 2024-11-06T04:46:01Z- handleDepth()- Depth_Keller msg: 1730868361.22 epoch sec; seqNo:1594; depth 50.32 2024-11-06T04:46:01Z- handleDVL() - DVL msg epochMillisec: 1730868361.22 2024-11-06T04:46:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:46:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:46:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:46:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:46:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:46:01Z- handleDvl() - RDI_Pathfinder msg: 1730868361.22 sec; seqNo:576; #beams:0 2024-11-06T04:46:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:46:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , -0.05 2024-11-06T04:46:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868361.24 epoch sec; seqNo:1594; 4061687.21 north; 594462.34 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:46:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868358.03 2024-11-06T04:46:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:01Z- waiting for 1730868362.03, time is 1730868361.24 2024-11-06T04:46:01Z- AHRS_M2 msg: 1730868361.62 epoch sec; seqNo:1482 2024-11-06T04:46:01Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.55 psi 2024-11-06T04:46:01Z- handleDepth()- Depth_Keller msg: 1730868361.63 epoch sec; seqNo:1595; depth 50.31 2024-11-06T04:46:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868361.64 epoch sec; seqNo:1595; 4061687.22 north; 594463.56 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:46:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868358.03 2024-11-06T04:46:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:01Z- waiting for 1730868362.03, time is 1730868361.64 2024-11-06T04:46:02Z- AHRS_M2 msg: 1730868362.03 epoch sec; seqNo:1483 2024-11-06T04:46:02Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.55 psi 2024-11-06T04:46:02Z- handleDepth()- Depth_Keller msg: 1730868362.05 epoch sec; seqNo:1596; depth 50.31 2024-11-06T04:46:02Z- handleDVL() - DVL msg epochMillisec: 1730868362.07 2024-11-06T04:46:02Z- handleDvl() - RDI_Pathfinder msg: 1730868362.07 sec; seqNo:577; #beams:4 2024-11-06T04:46:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.85 , 29.21 , 31.50 , 31.96 2024-11-06T04:46:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , -0.03 2024-11-06T04:46:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868362.10 epoch sec; seqNo:1596; 4061687.22 north; 594463.56 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:46:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868358.03 2024-11-06T04:46:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:02Z- waiting for 1730868362.03, time is 1730868362.10 2024-11-06T04:46:02Z- LcmTrn::updateTrn() >>>> pose time 1730868362.10, meas time 1730868362.07 <<<< 2024-11-06T04:46:02Z- LcmTrn::updateTrn() equating pose 1730868362.07 and meas times 1730868362.07 2024-11-06T04:46:02Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868362.07 2024-11-06T04:46:02Z- TerrainNav::motionUpdate() - currEstimate = (4061687, 594463.56, 50.31) 2024-11-06T04:46:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:46:02Z- TerrainNav:: SubcloudNIS is 0.66, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:46:02Z- TNavPF::Projecting Measurements 2024-11-06T04:46:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:46:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:46:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:46:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:46:02Z- TNavFilter::4 good beams. 2024-11-06T04:46:02Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:46:02Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:46:02Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:46:02Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:46:02Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:46:02Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:46:02Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:46:02Z- TerrainMapOctree: (vehN, vehE ) = (4061687.22, 594463.56). 2024-11-06T04:46:02Z- TerrainMapOctree: Min Distance = 222.59. 2024-11-06T04:46:02Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:46:02Z- Forcing Subcloud Comparison 2024-11-06T04:46:02Z- Weighting particles with subcloud comparison 2024-11-06T04:46:02Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:02Z- meanExpectedMeasDiff: 0.121339 delta_rms_squared: 0.904193 alpha: 0.978359 2024-11-06T04:46:02Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:02Z- meanExpectedMeasDiff: -2.723763 delta_rms_squared: 0.633458 alpha: 0.969394 2024-11-06T04:46:02Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:02Z- meanExpectedMeasDiff: -2.979712 delta_rms_squared: 0.740375 alpha: 0.973697 2024-11-06T04:46:02Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:02Z- meanExpectedMeasDiff: 1.712555 delta_rms_squared: 0.733450 alpha: 0.973455 2024-11-06T04:46:02Z- Subcloud NIS: 0.655817 2024-11-06T04:46:02Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:46:02Z- TNavPF:: sumWeights = 0.052961 2024-11-06T04:46:02Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:46:02Z- TNavPF::First Term of Map Covariance Matrix is 0.05 2024-11-06T04:46:02Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.01 2024-11-06T04:46:02Z- TerrainNav::Resampling particle filter... 2024-11-06T04:46:02Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868362.07 sec, ping # 0. 2024-11-06T04:46:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:46:02Z- variances([0], [2], and psi_berg) = 0.251077 0.420009 0.000000 2024-11-06T04:46:02Z- publishEstimatesDecoder() - at 1730868362.07 sec 2024-11-06T04:46:02Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:46:02Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061631.31 594467.27 56.78 2024-11-06T04:46:02Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061630.93 594467.97 57.36 lat/lon: 36.70 -121.94 2024-11-06T04:46:02Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.42 0.31 0.00 2024-11-06T04:46:02Z- AHRS_M2 msg: 1730868362.43 epoch sec; seqNo:1484 2024-11-06T04:46:02Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.55 psi 2024-11-06T04:46:02Z- handleDepth()- Depth_Keller msg: 1730868362.44 epoch sec; seqNo:1597; depth 50.31 2024-11-06T04:46:02Z- handleDVL() - DVL msg epochMillisec: 1730868362.44 2024-11-06T04:46:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:46:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:46:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:46:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:46:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:46:02Z- handleDvl() - RDI_Pathfinder msg: 1730868362.44 sec; seqNo:578; #beams:0 2024-11-06T04:46:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:46:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , -0.03 2024-11-06T04:46:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868362.46 epoch sec; seqNo:1597; 4061687.22 north; 594463.56 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:46:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868362.07 2024-11-06T04:46:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:02Z- waiting for 1730868366.07, time is 1730868362.46 2024-11-06T04:46:02Z- AHRS_M2 msg: 1730868362.84 epoch sec; seqNo:1485 2024-11-06T04:46:02Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.55 psi 2024-11-06T04:46:02Z- handleDepth()- Depth_Keller msg: 1730868362.85 epoch sec; seqNo:1598; depth 50.30 2024-11-06T04:46:02Z- handleDVL() - DVL msg epochMillisec: 1730868362.88 2024-11-06T04:46:02Z- handleDvl() - RDI_Pathfinder msg: 1730868362.88 sec; seqNo:579; #beams:4 2024-11-06T04:46:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.01 , 29.21 , 31.35 , 31.96 2024-11-06T04:46:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , -0.02 2024-11-06T04:46:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868362.90 epoch sec; seqNo:1598; 4061687.22 north; 594463.56 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:46:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868362.07 2024-11-06T04:46:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:03Z- waiting for 1730868366.07, time is 1730868362.90 2024-11-06T04:46:03Z- handleDepth()- Depth_Keller msg: 1730868363.26 epoch sec; seqNo:1599; depth 50.30 2024-11-06T04:46:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868363.28 epoch sec; seqNo:1599; 4061687.23 north; 594464.78 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:46:03Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:46:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868362.07 2024-11-06T04:46:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:03Z- waiting for 1730868366.07, time is 1730868363.28 2024-11-06T04:46:03Z- AHRS_M2 msg: 1730868363.64 epoch sec; seqNo:1486 2024-11-06T04:46:03Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.55 psi 2024-11-06T04:46:03Z- handleDepth()- Depth_Keller msg: 1730868363.66 epoch sec; seqNo:1600; depth 50.31 2024-11-06T04:46:03Z- handleDVL() - DVL msg epochMillisec: 1730868363.66 2024-11-06T04:46:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:46:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:46:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:46:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:46:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:46:03Z- handleDvl() - RDI_Pathfinder msg: 1730868363.66 sec; seqNo:580; #beams:0 2024-11-06T04:46:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:46:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , -0.02 2024-11-06T04:46:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868363.67 epoch sec; seqNo:1600; 4061687.23 north; 594464.78 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:46:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868362.07 2024-11-06T04:46:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:03Z- waiting for 1730868366.07, time is 1730868363.67 2024-11-06T04:46:04Z- AHRS_M2 msg: 1730868364.06 epoch sec; seqNo:1487 2024-11-06T04:46:04Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.55 psi 2024-11-06T04:46:04Z- handleDepth()- Depth_Keller msg: 1730868364.07 epoch sec; seqNo:1601; depth 50.29 2024-11-06T04:46:04Z- handleDVL() - DVL msg epochMillisec: 1730868364.11 2024-11-06T04:46:04Z- handleDvl() - RDI_Pathfinder msg: 1730868364.11 sec; seqNo:581; #beams:4 2024-11-06T04:46:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.85 , 29.36 , 31.50 , 31.96 2024-11-06T04:46:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.06 , -0.00 2024-11-06T04:46:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868364.12 epoch sec; seqNo:1601; 4061687.61 north; 594464.77 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:46:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868362.07 2024-11-06T04:46:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:04Z- waiting for 1730868366.07, time is 1730868364.12 2024-11-06T04:46:04Z- AHRS_M2 msg: 1730868364.46 epoch sec; seqNo:1488 2024-11-06T04:46:04Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.55 psi 2024-11-06T04:46:04Z- handleDepth()- Depth_Keller msg: 1730868364.46 epoch sec; seqNo:1602; depth 50.29 2024-11-06T04:46:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868364.48 epoch sec; seqNo:1602; 4061687.61 north; 594464.77 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:46:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868362.07 2024-11-06T04:46:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:04Z- waiting for 1730868366.07, time is 1730868364.48 2024-11-06T04:46:04Z- AHRS_M2 msg: 1730868364.86 epoch sec; seqNo:1489 2024-11-06T04:46:04Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.55 psi 2024-11-06T04:46:04Z- handleDepth()- Depth_Keller msg: 1730868364.87 epoch sec; seqNo:1603; depth 50.30 2024-11-06T04:46:04Z- handleDVL() - DVL msg epochMillisec: 1730868364.89 2024-11-06T04:46:04Z- handleDvl() - RDI_Pathfinder msg: 1730868364.89 sec; seqNo:582; #beams:4 2024-11-06T04:46:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.70 , 29.36 , 31.50 , 32.11 2024-11-06T04:46:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.05 , 0.02 2024-11-06T04:46:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868364.91 epoch sec; seqNo:1603; 4061687.63 north; 594465.99 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:46:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868362.07 2024-11-06T04:46:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:05Z- waiting for 1730868366.07, time is 1730868364.91 2024-11-06T04:46:05Z- AHRS_M2 msg: 1730868365.27 epoch sec; seqNo:1490 2024-11-06T04:46:05Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.55 psi 2024-11-06T04:46:05Z- handleDepth()- Depth_Keller msg: 1730868365.27 epoch sec; seqNo:1604; depth 50.30 2024-11-06T04:46:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868365.29 epoch sec; seqNo:1604; 4061687.63 north; 594465.99 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:46:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868362.07 2024-11-06T04:46:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:05Z- waiting for 1730868366.07, time is 1730868365.29 2024-11-06T04:46:05Z- AHRS_M2 msg: 1730868365.66 epoch sec; seqNo:1491 2024-11-06T04:46:05Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.55 psi 2024-11-06T04:46:05Z- handleDepth()- Depth_Keller msg: 1730868365.67 epoch sec; seqNo:1605; depth 50.31 2024-11-06T04:46:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868365.70 epoch sec; seqNo:1605; 4061687.63 north; 594465.99 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:46:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868362.07 2024-11-06T04:46:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:05Z- waiting for 1730868366.07, time is 1730868365.70 2024-11-06T04:46:06Z- AHRS_M2 msg: 1730868366.07 epoch sec; seqNo:1492 2024-11-06T04:46:06Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.55 psi 2024-11-06T04:46:06Z- handleDepth()- Depth_Keller msg: 1730868366.08 epoch sec; seqNo:1606; depth 50.31 2024-11-06T04:46:06Z- handleDVL() - DVL msg epochMillisec: 1730868366.10 2024-11-06T04:46:06Z- handleDvl() - RDI_Pathfinder msg: 1730868366.10 sec; seqNo:583; #beams:4 2024-11-06T04:46:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.39 , 29.51 , 31.50 , 32.26 2024-11-06T04:46:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , 0.03 2024-11-06T04:46:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868366.12 epoch sec; seqNo:1606; 4061687.64 north; 594467.21 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:46:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868362.07 2024-11-06T04:46:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:06Z- waiting for 1730868366.07, time is 1730868366.12 2024-11-06T04:46:06Z- LcmTrn::updateTrn() >>>> pose time 1730868366.12, meas time 1730868366.10 <<<< 2024-11-06T04:46:06Z- LcmTrn::updateTrn() equating pose 1730868366.10 and meas times 1730868366.10 2024-11-06T04:46:06Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868366.10 2024-11-06T04:46:06Z- TerrainNav::motionUpdate() - currEstimate = (4061688, 594467.21, 50.31) 2024-11-06T04:46:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:46:06Z- TerrainNav:: SubcloudNIS is 0.66, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:46:06Z- TNavPF::Projecting Measurements 2024-11-06T04:46:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:46:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:46:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:46:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:46:06Z- TNavFilter::4 good beams. 2024-11-06T04:46:06Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:46:06Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:46:06Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:46:06Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:46:06Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:46:06Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:46:06Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:46:06Z- TerrainMapOctree: (vehN, vehE ) = (4061687.64, 594467.21). 2024-11-06T04:46:06Z- TerrainMapOctree: Min Distance = 225.99. 2024-11-06T04:46:06Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:46:06Z- Forcing Subcloud Comparison 2024-11-06T04:46:06Z- Weighting particles with subcloud comparison 2024-11-06T04:46:06Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:06Z- meanExpectedMeasDiff: -0.082418 delta_rms_squared: 1.631799 alpha: 0.987892 2024-11-06T04:46:06Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:06Z- meanExpectedMeasDiff: -1.983367 delta_rms_squared: 0.625374 alpha: 0.969010 2024-11-06T04:46:06Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:06Z- meanExpectedMeasDiff: -2.806841 delta_rms_squared: 0.743824 alpha: 0.973816 2024-11-06T04:46:06Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:06Z- meanExpectedMeasDiff: 1.393500 delta_rms_squared: 0.588067 alpha: 0.967109 2024-11-06T04:46:06Z- Subcloud NIS: 0.637014 2024-11-06T04:46:06Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:46:06Z- TNavPF:: sumWeights = 0.118464 2024-11-06T04:46:06Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:46:06Z- TNavPF::First Term of Map Covariance Matrix is 0.13 2024-11-06T04:46:06Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.01 2024-11-06T04:46:06Z- TerrainNav::Resampling particle filter... 2024-11-06T04:46:06Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868366.10 sec, ping # 0. 2024-11-06T04:46:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:46:06Z- variances([0], [2], and psi_berg) = 0.255668 0.425829 0.000000 2024-11-06T04:46:06Z- publishEstimatesDecoder() - at 1730868366.10 sec 2024-11-06T04:46:06Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:46:06Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061631.80 594470.88 57.33 2024-11-06T04:46:06Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061631.35 594471.73 57.78 lat/lon: 36.70 -121.94 2024-11-06T04:46:06Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.43 0.30 0.00 2024-11-06T04:46:06Z- AHRS_M2 msg: 1730868366.47 epoch sec; seqNo:1493 2024-11-06T04:46:06Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.55 psi 2024-11-06T04:46:06Z- handleDepth()- Depth_Keller msg: 1730868366.48 epoch sec; seqNo:1607; depth 50.30 2024-11-06T04:46:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868366.51 epoch sec; seqNo:1607; 4061687.64 north; 594467.21 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:46:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868366.10 2024-11-06T04:46:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:06Z- waiting for 1730868370.10, time is 1730868366.51 2024-11-06T04:46:06Z- AHRS_M2 msg: 1730868366.88 epoch sec; seqNo:1494 2024-11-06T04:46:06Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.55 psi 2024-11-06T04:46:06Z- handleDepth()- Depth_Keller msg: 1730868366.88 epoch sec; seqNo:1608; depth 50.30 2024-11-06T04:46:06Z- handleDVL() - DVL msg epochMillisec: 1730868366.91 2024-11-06T04:46:06Z- handleDvl() - RDI_Pathfinder msg: 1730868366.91 sec; seqNo:584; #beams:4 2024-11-06T04:46:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.55 , 29.67 , 31.96 , 32.42 2024-11-06T04:46:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.05 2024-11-06T04:46:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868366.93 epoch sec; seqNo:1608; 4061687.64 north; 594467.21 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:46:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868366.10 2024-11-06T04:46:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:07Z- waiting for 1730868370.10, time is 1730868366.93 2024-11-06T04:46:07Z- AHRS_M2 msg: 1730868367.29 epoch sec; seqNo:1495 2024-11-06T04:46:07Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.55 psi 2024-11-06T04:46:07Z- handleDepth()- Depth_Keller msg: 1730868367.30 epoch sec; seqNo:1609; depth 50.32 2024-11-06T04:46:07Z- handleDVL() - DVL msg epochMillisec: 1730868367.30 2024-11-06T04:46:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:46:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:46:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:46:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:46:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:46:07Z- handleDvl() - RDI_Pathfinder msg: 1730868367.30 sec; seqNo:585; #beams:0 2024-11-06T04:46:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:46:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.05 2024-11-06T04:46:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868367.32 epoch sec; seqNo:1609; 4061687.64 north; 594467.21 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:46:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868366.10 2024-11-06T04:46:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:07Z- waiting for 1730868370.10, time is 1730868367.32 2024-11-06T04:46:07Z- AHRS_M2 msg: 1730868367.68 epoch sec; seqNo:1496 2024-11-06T04:46:07Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.55 psi 2024-11-06T04:46:07Z- handleDepth()- Depth_Keller msg: 1730868367.69 epoch sec; seqNo:1610; depth 50.31 2024-11-06T04:46:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868367.70 epoch sec; seqNo:1610; 4061687.65 north; 594468.43 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:46:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868366.10 2024-11-06T04:46:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:07Z- waiting for 1730868370.10, time is 1730868367.70 2024-11-06T04:46:08Z- AHRS_M2 msg: 1730868368.09 epoch sec; seqNo:1497 2024-11-06T04:46:08Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.55 psi 2024-11-06T04:46:08Z- handleDepth()- Depth_Keller msg: 1730868368.09 epoch sec; seqNo:1611; depth 50.31 2024-11-06T04:46:08Z- handleDVL() - DVL msg epochMillisec: 1730868368.13 2024-11-06T04:46:08Z- handleDvl() - RDI_Pathfinder msg: 1730868368.13 sec; seqNo:586; #beams:4 2024-11-06T04:46:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.85 , 29.51 , 32.42 , 32.57 2024-11-06T04:46:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.05 2024-11-06T04:46:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868368.14 epoch sec; seqNo:1611; 4061687.65 north; 594468.43 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:46:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868366.10 2024-11-06T04:46:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:08Z- waiting for 1730868370.10, time is 1730868368.14 2024-11-06T04:46:08Z- AHRS_M2 msg: 1730868368.49 epoch sec; seqNo:1498 2024-11-06T04:46:08Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.55 psi 2024-11-06T04:46:08Z- handleDepth()- Depth_Keller msg: 1730868368.49 epoch sec; seqNo:1612; depth 50.30 2024-11-06T04:46:08Z- handleDVL() - DVL msg epochMillisec: 1730868368.50 2024-11-06T04:46:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:46:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:46:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:46:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:46:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:46:08Z- handleDvl() - RDI_Pathfinder msg: 1730868368.50 sec; seqNo:587; #beams:0 2024-11-06T04:46:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:46:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.05 2024-11-06T04:46:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868368.51 epoch sec; seqNo:1612; 4061687.65 north; 594468.43 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:46:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868366.10 2024-11-06T04:46:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:08Z- waiting for 1730868370.10, time is 1730868368.51 2024-11-06T04:46:08Z- AHRS_M2 msg: 1730868368.90 epoch sec; seqNo:1499 2024-11-06T04:46:08Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.55 psi 2024-11-06T04:46:08Z- handleDepth()- Depth_Keller msg: 1730868368.91 epoch sec; seqNo:1613; depth 50.30 2024-11-06T04:46:08Z- handleDVL() - DVL msg epochMillisec: 1730868368.93 2024-11-06T04:46:08Z- handleDvl() - RDI_Pathfinder msg: 1730868368.93 sec; seqNo:588; #beams:4 2024-11-06T04:46:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.85 , 29.67 , 33.33 , 32.88 2024-11-06T04:46:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.05 2024-11-06T04:46:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868368.94 epoch sec; seqNo:1613; 4061687.65 north; 594468.43 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:46:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868366.10 2024-11-06T04:46:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:09Z- waiting for 1730868370.10, time is 1730868368.94 2024-11-06T04:46:09Z- AHRS_M2 msg: 1730868369.30 epoch sec; seqNo:1500 2024-11-06T04:46:09Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.55 psi 2024-11-06T04:46:09Z- handleDepth()- Depth_Keller msg: 1730868369.32 epoch sec; seqNo:1614; depth 50.30 2024-11-06T04:46:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868369.34 epoch sec; seqNo:1614; 4061688.05 north; 594469.65 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:46:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868366.10 2024-11-06T04:46:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:09Z- waiting for 1730868370.10, time is 1730868369.34 2024-11-06T04:46:09Z- AHRS_M2 msg: 1730868369.70 epoch sec; seqNo:1501 2024-11-06T04:46:09Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.55 psi 2024-11-06T04:46:09Z- handleDepth()- Depth_Keller msg: 1730868369.71 epoch sec; seqNo:1615; depth 50.30 2024-11-06T04:46:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868369.72 epoch sec; seqNo:1615; 4061688.05 north; 594469.65 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:46:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868366.10 2024-11-06T04:46:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:09Z- waiting for 1730868370.10, time is 1730868369.72 2024-11-06T04:46:10Z- AHRS_M2 msg: 1730868370.11 epoch sec; seqNo:1502 2024-11-06T04:46:10Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.55 psi 2024-11-06T04:46:10Z- handleDepth()- Depth_Keller msg: 1730868370.11 epoch sec; seqNo:1616; depth 50.28 2024-11-06T04:46:10Z- handleDVL() - DVL msg epochMillisec: 1730868370.13 2024-11-06T04:46:10Z- handleDvl() - RDI_Pathfinder msg: 1730868370.13 sec; seqNo:589; #beams:4 2024-11-06T04:46:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.85 , 29.67 , 34.25 , 32.57 2024-11-06T04:46:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.03 2024-11-06T04:46:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868370.16 epoch sec; seqNo:1616; 4061688.05 north; 594469.65 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T04:46:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868366.10 2024-11-06T04:46:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:10Z- waiting for 1730868370.10, time is 1730868370.16 2024-11-06T04:46:10Z- LcmTrn::updateTrn() >>>> pose time 1730868370.16, meas time 1730868370.13 <<<< 2024-11-06T04:46:10Z- LcmTrn::updateTrn() equating pose 1730868370.13 and meas times 1730868370.13 2024-11-06T04:46:10Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868370.13 2024-11-06T04:46:10Z- TerrainNav::motionUpdate() - currEstimate = (4061688, 594469.65, 50.28) 2024-11-06T04:46:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:46:10Z- TerrainNav:: SubcloudNIS is 0.64, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:46:10Z- TNavPF::Projecting Measurements 2024-11-06T04:46:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:46:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:46:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:46:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:46:10Z- TNavFilter::4 good beams. 2024-11-06T04:46:10Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:46:10Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:46:10Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:46:10Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:46:10Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:46:10Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:46:10Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:46:10Z- TerrainMapOctree: (vehN, vehE ) = (4061688.05, 594469.65). 2024-11-06T04:46:10Z- TerrainMapOctree: Min Distance = 228.22. 2024-11-06T04:46:10Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:46:10Z- Forcing Subcloud Comparison 2024-11-06T04:46:10Z- Weighting particles with subcloud comparison 2024-11-06T04:46:10Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:10Z- meanExpectedMeasDiff: -0.104480 delta_rms_squared: 1.003649 alpha: 0.980462 2024-11-06T04:46:10Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:10Z- meanExpectedMeasDiff: -1.432413 delta_rms_squared: 0.636484 alpha: 0.969535 2024-11-06T04:46:10Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:10Z- meanExpectedMeasDiff: 0.147181 delta_rms_squared: 0.767567 alpha: 0.974605 2024-11-06T04:46:10Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:10Z- meanExpectedMeasDiff: 1.352818 delta_rms_squared: 0.642548 alpha: 0.969814 2024-11-06T04:46:10Z- Subcloud NIS: 0.593076 2024-11-06T04:46:10Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:46:10Z- TNavPF:: sumWeights = 0.411051 2024-11-06T04:46:10Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:46:10Z- TNavPF::First Term of Map Covariance Matrix is 0.22 2024-11-06T04:46:10Z- TNavPF::Calculated NIS Value : 0.31 numBeams normalized NIS: 0.08 2024-11-06T04:46:10Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868370.13 sec, ping # 0. 2024-11-06T04:46:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:46:10Z- variances([0], [2], and psi_berg) = 0.252751 0.440830 0.000000 2024-11-06T04:46:10Z- publishEstimatesDecoder() - at 1730868370.13 sec 2024-11-06T04:46:10Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:46:10Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061631.56 594475.53 58.05 2024-11-06T04:46:10Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061631.75 594474.19 57.77 lat/lon: 36.70 -121.94 2024-11-06T04:46:10Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.44 0.31 0.00 2024-11-06T04:46:10Z- AHRS_M2 msg: 1730868370.51 epoch sec; seqNo:1503 2024-11-06T04:46:10Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.55 psi 2024-11-06T04:46:10Z- handleDepth()- Depth_Keller msg: 1730868370.52 epoch sec; seqNo:1617; depth 50.28 2024-11-06T04:46:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868370.53 epoch sec; seqNo:1617; 4061688.06 north; 594470.87 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T04:46:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868370.13 2024-11-06T04:46:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:10Z- waiting for 1730868374.13, time is 1730868370.53 2024-11-06T04:46:10Z- AHRS_M2 msg: 1730868370.91 epoch sec; seqNo:1504 2024-11-06T04:46:10Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.55 psi 2024-11-06T04:46:10Z- handleDepth()- Depth_Keller msg: 1730868370.92 epoch sec; seqNo:1618; depth 50.26 2024-11-06T04:46:10Z- handleDVL() - DVL msg epochMillisec: 1730868370.95 2024-11-06T04:46:10Z- handleDvl() - RDI_Pathfinder msg: 1730868370.95 sec; seqNo:590; #beams:4 2024-11-06T04:46:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.85 , 29.67 , 35.02 , 32.72 2024-11-06T04:46:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.00 2024-11-06T04:46:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868370.96 epoch sec; seqNo:1618; 4061688.06 north; 594470.87 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:46:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868370.13 2024-11-06T04:46:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:11Z- waiting for 1730868374.13, time is 1730868370.96 2024-11-06T04:46:11Z- AHRS_M2 msg: 1730868371.33 epoch sec; seqNo:1505 2024-11-06T04:46:11Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.55 psi 2024-11-06T04:46:11Z- handleDepth()- Depth_Keller msg: 1730868371.33 epoch sec; seqNo:1619; depth 50.24 2024-11-06T04:46:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868371.37 epoch sec; seqNo:1619; 4061688.06 north; 594470.87 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:46:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868370.13 2024-11-06T04:46:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:11Z- waiting for 1730868374.13, time is 1730868371.37 2024-11-06T04:46:11Z- AHRS_M2 msg: 1730868371.72 epoch sec; seqNo:1506 2024-11-06T04:46:11Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.56 psi 2024-11-06T04:46:11Z- handleDepth()- Depth_Keller msg: 1730868371.73 epoch sec; seqNo:1620; depth 50.24 2024-11-06T04:46:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868371.74 epoch sec; seqNo:1620; 4061688.07 north; 594472.09 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:46:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868370.13 2024-11-06T04:46:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:11Z- waiting for 1730868374.13, time is 1730868371.74 2024-11-06T04:46:12Z- AHRS_M2 msg: 1730868372.13 epoch sec; seqNo:1507 2024-11-06T04:46:12Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.56 psi 2024-11-06T04:46:12Z- handleDepth()- Depth_Keller msg: 1730868372.14 epoch sec; seqNo:1621; depth 50.24 2024-11-06T04:46:12Z- handleDVL() - DVL msg epochMillisec: 1730868372.16 2024-11-06T04:46:12Z- handleDvl() - RDI_Pathfinder msg: 1730868372.16 sec; seqNo:591; #beams:4 2024-11-06T04:46:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.39 , 29.51 , 35.32 , 32.72 2024-11-06T04:46:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.00 , -0.03 2024-11-06T04:46:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868372.17 epoch sec; seqNo:1621; 4061688.07 north; 594472.09 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:46:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868370.13 2024-11-06T04:46:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:12Z- waiting for 1730868374.13, time is 1730868372.17 2024-11-06T04:46:12Z- AHRS_M2 msg: 1730868372.53 epoch sec; seqNo:1508 2024-11-06T04:46:12Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.56 psi 2024-11-06T04:46:12Z- handleDepth()- Depth_Keller msg: 1730868372.54 epoch sec; seqNo:1622; depth 50.21 2024-11-06T04:46:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868372.56 epoch sec; seqNo:1622; 4061688.07 north; 594472.09 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T04:46:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868370.13 2024-11-06T04:46:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:12Z- waiting for 1730868374.13, time is 1730868372.56 2024-11-06T04:46:12Z- handleDepth()- Depth_Keller msg: 1730868372.96 epoch sec; seqNo:1623; depth 50.19 2024-11-06T04:46:12Z- handleDVL() - DVL msg epochMillisec: 1730868372.98 2024-11-06T04:46:12Z- handleDvl() - RDI_Pathfinder msg: 1730868372.98 sec; seqNo:592; #beams:4 2024-11-06T04:46:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.94 , 29.67 , 35.32 , 32.57 2024-11-06T04:46:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.04 2024-11-06T04:46:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868372.99 epoch sec; seqNo:1623; 4061688.09 north; 594473.31 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:46:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868370.13 2024-11-06T04:46:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:13Z- waiting for 1730868374.13, time is 1730868372.99 2024-11-06T04:46:13Z- AHRS_M2 msg: 1730868373.34 epoch sec; seqNo:1509 2024-11-06T04:46:13Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.56 psi 2024-11-06T04:46:13Z- handleDepth()- Depth_Keller msg: 1730868373.35 epoch sec; seqNo:1624; depth 50.17 2024-11-06T04:46:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868373.36 epoch sec; seqNo:1624; 4061688.09 north; 594473.31 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T04:46:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868370.13 2024-11-06T04:46:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:13Z- waiting for 1730868374.13, time is 1730868373.36 2024-11-06T04:46:13Z- AHRS_M2 msg: 1730868373.74 epoch sec; seqNo:1510 2024-11-06T04:46:13Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.56 psi 2024-11-06T04:46:13Z- handleDepth()- Depth_Keller msg: 1730868373.75 epoch sec; seqNo:1625; depth 50.15 2024-11-06T04:46:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868373.76 epoch sec; seqNo:1625; 4061688.09 north; 594473.31 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T04:46:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868370.13 2024-11-06T04:46:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:13Z- waiting for 1730868374.13, time is 1730868373.76 2024-11-06T04:46:14Z- handleDepth()- Depth_Keller msg: 1730868374.17 epoch sec; seqNo:1626; depth 50.12 2024-11-06T04:46:14Z- handleDVL() - DVL msg epochMillisec: 1730868374.19 2024-11-06T04:46:14Z- handleDvl() - RDI_Pathfinder msg: 1730868374.19 sec; seqNo:593; #beams:4 2024-11-06T04:46:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.78 , 29.21 , 35.94 , 32.42 2024-11-06T04:46:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , -0.04 2024-11-06T04:46:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868374.20 epoch sec; seqNo:1626; 4061688.09 north; 594473.31 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T04:46:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868370.13 2024-11-06T04:46:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:14Z- waiting for 1730868374.13, time is 1730868374.20 2024-11-06T04:46:14Z- LcmTrn::updateTrn() >>>> pose time 1730868374.20, meas time 1730868374.19 <<<< 2024-11-06T04:46:14Z- LcmTrn::updateTrn() equating pose 1730868374.19 and meas times 1730868374.19 2024-11-06T04:46:14Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868374.19 2024-11-06T04:46:14Z- TerrainNav::motionUpdate() - currEstimate = (4061688, 594473.31, 50.12) 2024-11-06T04:46:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:46:14Z- TerrainNav:: SubcloudNIS is 0.59, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:46:14Z- TNavPF::Projecting Measurements 2024-11-06T04:46:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:46:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:46:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:46:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:46:14Z- TNavFilter::4 good beams. 2024-11-06T04:46:14Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:46:14Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:46:14Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:46:14Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:46:14Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:46:14Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:46:14Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:46:14Z- TerrainMapOctree: (vehN, vehE ) = (4061688.09, 594473.31). 2024-11-06T04:46:14Z- TerrainMapOctree: Min Distance = 231.73. 2024-11-06T04:46:14Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:46:14Z- Forcing Subcloud Comparison 2024-11-06T04:46:14Z- Weighting particles with subcloud comparison 2024-11-06T04:46:14Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:14Z- meanExpectedMeasDiff: -1.050159 delta_rms_squared: 0.891239 alpha: 0.978052 2024-11-06T04:46:14Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:14Z- meanExpectedMeasDiff: -2.202328 delta_rms_squared: 0.645395 alpha: 0.969943 2024-11-06T04:46:14Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:14Z- meanExpectedMeasDiff: 1.588768 delta_rms_squared: 0.773690 alpha: 0.974801 2024-11-06T04:46:14Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:14Z- meanExpectedMeasDiff: 0.094024 delta_rms_squared: 0.733038 alpha: 0.973441 2024-11-06T04:46:14Z- Subcloud NIS: 0.566636 2024-11-06T04:46:14Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:46:14Z- TNavPF:: sumWeights = 0.226312 2024-11-06T04:46:14Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:46:14Z- TNavPF::First Term of Map Covariance Matrix is 0.20 2024-11-06T04:46:14Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T04:46:14Z- TerrainNav::Resampling particle filter... 2024-11-06T04:46:14Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868374.19 sec, ping # 0. 2024-11-06T04:46:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:46:14Z- variances([0], [2], and psi_berg) = 0.252649 0.443443 0.000000 2024-11-06T04:46:14Z- publishEstimatesDecoder() - at 1730868374.19 sec 2024-11-06T04:46:14Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:46:14Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061632.31 594477.05 56.71 2024-11-06T04:46:14Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061631.79 594477.86 57.78 lat/lon: 36.70 -121.94 2024-11-06T04:46:14Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.25 0.44 0.31 0.00 2024-11-06T04:46:14Z- AHRS_M2 msg: 1730868374.55 epoch sec; seqNo:1511 2024-11-06T04:46:14Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.56 psi 2024-11-06T04:46:14Z- handleDepth()- Depth_Keller msg: 1730868374.56 epoch sec; seqNo:1627; depth 50.09 2024-11-06T04:46:14Z- handleDVL() - DVL msg epochMillisec: 1730868374.56 2024-11-06T04:46:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:46:14Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:46:14Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:46:14Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:46:14Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:46:14Z- handleDvl() - RDI_Pathfinder msg: 1730868374.56 sec; seqNo:594; #beams:0 2024-11-06T04:46:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:46:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , -0.04 2024-11-06T04:46:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868374.60 epoch sec; seqNo:1627; 4061688.10 north; 594474.53 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T04:46:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868374.19 2024-11-06T04:46:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:14Z- waiting for 1730868378.19, time is 1730868374.60 2024-11-06T04:46:14Z- handleDepth()- Depth_Keller msg: 1730868374.98 epoch sec; seqNo:1628; depth 50.07 2024-11-06T04:46:15Z- handleDVL() - DVL msg epochMillisec: 1730868375.01 2024-11-06T04:46:15Z- handleDvl() - RDI_Pathfinder msg: 1730868375.01 sec; seqNo:595; #beams:4 2024-11-06T04:46:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.78 , 29.36 , 36.24 , 32.42 2024-11-06T04:46:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.04 , -0.03 2024-11-06T04:46:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868375.03 epoch sec; seqNo:1628; 4061688.10 north; 594474.53 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T04:46:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868374.19 2024-11-06T04:46:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:15Z- waiting for 1730868378.19, time is 1730868375.03 2024-11-06T04:46:15Z- AHRS_M2 msg: 1730868375.36 epoch sec; seqNo:1512 2024-11-06T04:46:15Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.55 psi 2024-11-06T04:46:15Z- handleDepth()- Depth_Keller msg: 1730868375.37 epoch sec; seqNo:1629; depth 50.04 2024-11-06T04:46:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868375.38 epoch sec; seqNo:1629; 4061688.10 north; 594474.53 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T04:46:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868374.19 2024-11-06T04:46:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:15Z- waiting for 1730868378.19, time is 1730868375.38 2024-11-06T04:46:15Z- AHRS_M2 msg: 1730868375.76 epoch sec; seqNo:1513 2024-11-06T04:46:15Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.55 psi 2024-11-06T04:46:15Z- handleDepth()- Depth_Keller msg: 1730868375.77 epoch sec; seqNo:1630; depth 50.03 2024-11-06T04:46:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868375.78 epoch sec; seqNo:1630; 4061688.11 north; 594475.75 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T04:46:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868374.19 2024-11-06T04:46:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:15Z- waiting for 1730868378.19, time is 1730868375.78 2024-11-06T04:46:16Z- AHRS_M2 msg: 1730868376.17 epoch sec; seqNo:1514 2024-11-06T04:46:16Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.55 psi 2024-11-06T04:46:16Z- handleDepth()- Depth_Keller msg: 1730868376.17 epoch sec; seqNo:1631; depth 50.02 2024-11-06T04:46:16Z- handleDVL() - DVL msg epochMillisec: 1730868376.20 2024-11-06T04:46:16Z- handleDvl() - RDI_Pathfinder msg: 1730868376.20 sec; seqNo:596; #beams:4 2024-11-06T04:46:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 29.36 , 36.24 , 32.57 2024-11-06T04:46:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , -0.01 2024-11-06T04:46:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868376.21 epoch sec; seqNo:1631; 4061688.11 north; 594475.75 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:46:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868374.19 2024-11-06T04:46:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:16Z- waiting for 1730868378.19, time is 1730868376.21 2024-11-06T04:46:16Z- AHRS_M2 msg: 1730868376.57 epoch sec; seqNo:1515 2024-11-06T04:46:16Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.55 psi 2024-11-06T04:46:16Z- handleDepth()- Depth_Keller msg: 1730868376.58 epoch sec; seqNo:1632; depth 49.99 2024-11-06T04:46:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868376.60 epoch sec; seqNo:1632; 4061688.11 north; 594475.75 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T04:46:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868374.19 2024-11-06T04:46:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:16Z- waiting for 1730868378.19, time is 1730868376.60 2024-11-06T04:46:17Z- AHRS_M2 msg: 1730868376.99 epoch sec; seqNo:1516 2024-11-06T04:46:17Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.54 psi 2024-11-06T04:46:17Z- handleDepth()- Depth_Keller msg: 1730868377.00 epoch sec; seqNo:1633; depth 49.97 2024-11-06T04:46:17Z- handleDVL() - DVL msg epochMillisec: 1730868377.02 2024-11-06T04:46:17Z- handleDvl() - RDI_Pathfinder msg: 1730868377.02 sec; seqNo:597; #beams:4 2024-11-06T04:46:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 29.51 , 35.94 , 32.57 2024-11-06T04:46:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.05 , 0.01 2024-11-06T04:46:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868377.04 epoch sec; seqNo:1633; 4061688.11 north; 594475.75 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T04:46:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868374.19 2024-11-06T04:46:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:17Z- waiting for 1730868378.19, time is 1730868377.04 2024-11-06T04:46:17Z- AHRS_M2 msg: 1730868377.38 epoch sec; seqNo:1517 2024-11-06T04:46:17Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.54 psi 2024-11-06T04:46:17Z- handleDepth()- Depth_Keller msg: 1730868377.39 epoch sec; seqNo:1634; depth 49.96 2024-11-06T04:46:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868377.41 epoch sec; seqNo:1634; 4061688.13 north; 594476.97 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:46:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868374.19 2024-11-06T04:46:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:17Z- waiting for 1730868378.19, time is 1730868377.41 2024-11-06T04:46:17Z- AHRS_M2 msg: 1730868377.78 epoch sec; seqNo:1518 2024-11-06T04:46:17Z- AHRS_M2 msg: 0.11 phi; 0.02 theta; 1.53 psi 2024-11-06T04:46:17Z- handleDepth()- Depth_Keller msg: 1730868377.79 epoch sec; seqNo:1635; depth 49.93 2024-11-06T04:46:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868377.80 epoch sec; seqNo:1635; 4061688.13 north; 594476.97 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T04:46:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868374.19 2024-11-06T04:46:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:17Z- waiting for 1730868378.19, time is 1730868377.80 2024-11-06T04:46:18Z- AHRS_M2 msg: 1730868378.19 epoch sec; seqNo:1519 2024-11-06T04:46:18Z- AHRS_M2 msg: 0.11 phi; 0.02 theta; 1.53 psi 2024-11-06T04:46:18Z- handleDepth()- Depth_Keller msg: 1730868378.19 epoch sec; seqNo:1636; depth 49.92 2024-11-06T04:46:18Z- handleDVL() - DVL msg epochMillisec: 1730868378.21 2024-11-06T04:46:18Z- handleDvl() - RDI_Pathfinder msg: 1730868378.21 sec; seqNo:598; #beams:4 2024-11-06T04:46:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.48 , 29.82 , 35.94 , 32.42 2024-11-06T04:46:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.05 , 0.03 2024-11-06T04:46:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868378.24 epoch sec; seqNo:1636; 4061688.51 north; 594476.97 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:46:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868374.19 2024-11-06T04:46:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:18Z- waiting for 1730868378.19, time is 1730868378.24 2024-11-06T04:46:18Z- LcmTrn::updateTrn() >>>> pose time 1730868378.24, meas time 1730868378.21 <<<< 2024-11-06T04:46:18Z- LcmTrn::updateTrn() equating pose 1730868378.21 and meas times 1730868378.21 2024-11-06T04:46:18Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868378.21 2024-11-06T04:46:18Z- TerrainNav::motionUpdate() - currEstimate = (4061689, 594476.97, 49.92) 2024-11-06T04:46:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:46:18Z- TerrainNav:: SubcloudNIS is 0.57, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:46:18Z- TNavPF::Projecting Measurements 2024-11-06T04:46:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:46:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:46:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:46:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:46:18Z- TNavFilter::4 good beams. 2024-11-06T04:46:18Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:46:18Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:46:18Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:46:18Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:46:18Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:46:18Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:46:18Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:46:18Z- TerrainMapOctree: (vehN, vehE ) = (4061688.51, 594476.97). 2024-11-06T04:46:18Z- TerrainMapOctree: Min Distance = 235.15. 2024-11-06T04:46:18Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:46:18Z- Forcing Subcloud Comparison 2024-11-06T04:46:18Z- Weighting particles with subcloud comparison 2024-11-06T04:46:18Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:18Z- meanExpectedMeasDiff: 0.174839 delta_rms_squared: 1.677571 alpha: 0.988218 2024-11-06T04:46:18Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:18Z- meanExpectedMeasDiff: -1.964102 delta_rms_squared: 0.654234 alpha: 0.970337 2024-11-06T04:46:18Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:18Z- meanExpectedMeasDiff: 1.739205 delta_rms_squared: 0.758956 alpha: 0.974325 2024-11-06T04:46:18Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:18Z- meanExpectedMeasDiff: 0.162242 delta_rms_squared: 0.721780 alpha: 0.973038 2024-11-06T04:46:18Z- Subcloud NIS: 0.557931 2024-11-06T04:46:18Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:46:18Z- TNavPF:: sumWeights = 0.254765 2024-11-06T04:46:18Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:46:18Z- TNavPF::First Term of Map Covariance Matrix is 0.25 2024-11-06T04:46:18Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T04:46:18Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868378.21 sec, ping # 0. 2024-11-06T04:46:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:46:18Z- variances([0], [2], and psi_berg) = 0.257574 0.451143 0.000000 2024-11-06T04:46:18Z- publishEstimatesDecoder() - at 1730868378.21 sec 2024-11-06T04:46:18Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:46:18Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061632.42 594479.83 57.78 2024-11-06T04:46:18Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061632.21 594481.51 57.61 lat/lon: 36.70 -121.94 2024-11-06T04:46:18Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.26 0.45 0.28 0.00 2024-11-06T04:46:18Z- AHRS_M2 msg: 1730868378.59 epoch sec; seqNo:1520 2024-11-06T04:46:18Z- AHRS_M2 msg: 0.11 phi; 0.02 theta; 1.53 psi 2024-11-06T04:46:18Z- handleDepth()- Depth_Keller msg: 1730868378.60 epoch sec; seqNo:1637; depth 49.91 2024-11-06T04:46:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868378.63 epoch sec; seqNo:1637; 4061688.51 north; 594476.97 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:46:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868378.21 2024-11-06T04:46:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:18Z- waiting for 1730868382.21, time is 1730868378.63 2024-11-06T04:46:19Z- AHRS_M2 msg: 1730868379.02 epoch sec; seqNo:1521 2024-11-06T04:46:19Z- AHRS_M2 msg: 0.11 phi; 0.02 theta; 1.53 psi 2024-11-06T04:46:19Z- handleDepth()- Depth_Keller msg: 1730868379.03 epoch sec; seqNo:1638; depth 49.89 2024-11-06T04:46:19Z- handleDVL() - DVL msg epochMillisec: 1730868379.05 2024-11-06T04:46:19Z- handleDvl() - RDI_Pathfinder msg: 1730868379.05 sec; seqNo:599; #beams:4 2024-11-06T04:46:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.32 , 29.97 , 35.94 , 32.57 2024-11-06T04:46:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , 0.04 2024-11-06T04:46:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868379.07 epoch sec; seqNo:1638; 4061688.52 north; 594478.19 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:46:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868378.21 2024-11-06T04:46:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:19Z- waiting for 1730868382.21, time is 1730868379.07 2024-11-06T04:46:19Z- AHRS_M2 msg: 1730868379.40 epoch sec; seqNo:1522 2024-11-06T04:46:19Z- AHRS_M2 msg: 0.11 phi; 0.02 theta; 1.52 psi 2024-11-06T04:46:19Z- handleDepth()- Depth_Keller msg: 1730868379.41 epoch sec; seqNo:1639; depth 49.88 2024-11-06T04:46:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868379.42 epoch sec; seqNo:1639; 4061688.52 north; 594478.19 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T04:46:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868378.21 2024-11-06T04:46:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:19Z- waiting for 1730868382.21, time is 1730868379.42 2024-11-06T04:46:19Z- AHRS_M2 msg: 1730868379.81 epoch sec; seqNo:1523 2024-11-06T04:46:19Z- AHRS_M2 msg: 0.10 phi; 0.02 theta; 1.52 psi 2024-11-06T04:46:19Z- handleDepth()- Depth_Keller msg: 1730868379.82 epoch sec; seqNo:1640; depth 49.87 2024-11-06T04:46:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868379.83 epoch sec; seqNo:1640; 4061688.52 north; 594478.19 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:46:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868378.21 2024-11-06T04:46:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:19Z- waiting for 1730868382.21, time is 1730868379.83 2024-11-06T04:46:20Z- AHRS_M2 msg: 1730868380.21 epoch sec; seqNo:1524 2024-11-06T04:46:20Z- AHRS_M2 msg: 0.10 phi; 0.02 theta; 1.52 psi 2024-11-06T04:46:20Z- handleDepth()- Depth_Keller msg: 1730868380.22 epoch sec; seqNo:1641; depth 49.85 2024-11-06T04:46:20Z- handleDVL() - DVL msg epochMillisec: 1730868380.25 2024-11-06T04:46:20Z- handleDvl() - RDI_Pathfinder msg: 1730868380.25 sec; seqNo:600; #beams:4 2024-11-06T04:46:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.17 , 30.12 , 35.78 , 32.57 2024-11-06T04:46:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.04 , 0.03 2024-11-06T04:46:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868380.26 epoch sec; seqNo:1641; 4061688.52 north; 594478.19 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:46:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868378.21 2024-11-06T04:46:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:20Z- waiting for 1730868382.21, time is 1730868380.26 2024-11-06T04:46:20Z- AHRS_M2 msg: 1730868380.61 epoch sec; seqNo:1525 2024-11-06T04:46:20Z- AHRS_M2 msg: 0.11 phi; 0.02 theta; 1.52 psi 2024-11-06T04:46:20Z- handleDepth()- Depth_Keller msg: 1730868380.62 epoch sec; seqNo:1642; depth 49.83 2024-11-06T04:46:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868380.64 epoch sec; seqNo:1642; 4061688.53 north; 594479.41 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:46:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868378.21 2024-11-06T04:46:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:20Z- waiting for 1730868382.21, time is 1730868380.64 2024-11-06T04:46:21Z- AHRS_M2 msg: 1730868381.02 epoch sec; seqNo:1526 2024-11-06T04:46:21Z- AHRS_M2 msg: 0.11 phi; 0.02 theta; 1.52 psi 2024-11-06T04:46:21Z- handleDepth()- Depth_Keller msg: 1730868381.03 epoch sec; seqNo:1643; depth 49.83 2024-11-06T04:46:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868381.04 epoch sec; seqNo:1643; 4061688.53 north; 594479.41 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:46:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868378.21 2024-11-06T04:46:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:21Z- waiting for 1730868382.21, time is 1730868381.04 2024-11-06T04:46:21Z- AHRS_M2 msg: 1730868381.43 epoch sec; seqNo:1527 2024-11-06T04:46:21Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.52 psi 2024-11-06T04:46:21Z- handleDepth()- Depth_Keller msg: 1730868381.43 epoch sec; seqNo:1644; depth 49.81 2024-11-06T04:46:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868381.45 epoch sec; seqNo:1644; 4061688.53 north; 594479.41 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:46:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868378.21 2024-11-06T04:46:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:21Z- waiting for 1730868382.21, time is 1730868381.45 2024-11-06T04:46:21Z- AHRS_M2 msg: 1730868381.83 epoch sec; seqNo:1528 2024-11-06T04:46:21Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.52 psi 2024-11-06T04:46:21Z- handleDepth()- Depth_Keller msg: 1730868381.84 epoch sec; seqNo:1645; depth 49.81 2024-11-06T04:46:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868381.85 epoch sec; seqNo:1645; 4061688.55 north; 594480.63 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:46:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868378.21 2024-11-06T04:46:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:21Z- waiting for 1730868382.21, time is 1730868381.85 2024-11-06T04:46:22Z- handleDepth()- Depth_Keller msg: 1730868382.25 epoch sec; seqNo:1646; depth 49.79 2024-11-06T04:46:22Z- handleDVL() - DVL msg epochMillisec: 1730868382.27 2024-11-06T04:46:22Z- handleDvl() - RDI_Pathfinder msg: 1730868382.27 sec; seqNo:601; #beams:4 2024-11-06T04:46:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.24 , 30.89 , 36.70 , 32.88 2024-11-06T04:46:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.02 2024-11-06T04:46:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868382.29 epoch sec; seqNo:1646; 4061688.55 north; 594480.63 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:46:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868378.21 2024-11-06T04:46:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:22Z- waiting for 1730868382.21, time is 1730868382.29 2024-11-06T04:46:22Z- LcmTrn::updateTrn() >>>> pose time 1730868382.29, meas time 1730868382.27 <<<< 2024-11-06T04:46:22Z- LcmTrn::updateTrn() equating pose 1730868382.27 and meas times 1730868382.27 2024-11-06T04:46:22Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868382.27 2024-11-06T04:46:22Z- TerrainNav::motionUpdate() - currEstimate = (4061689, 594480.63, 49.79) 2024-11-06T04:46:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:46:22Z- TerrainNav:: SubcloudNIS is 0.56, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:46:22Z- TNavPF::Projecting Measurements 2024-11-06T04:46:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:46:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:46:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:46:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:46:22Z- TNavFilter::4 good beams. 2024-11-06T04:46:22Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:46:22Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:46:22Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:46:22Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:46:22Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:46:22Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:46:22Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:46:22Z- TerrainMapOctree: (vehN, vehE ) = (4061688.55, 594480.63). 2024-11-06T04:46:22Z- TerrainMapOctree: Min Distance = 238.68. 2024-11-06T04:46:22Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:46:22Z- Forcing Subcloud Comparison 2024-11-06T04:46:22Z- Weighting particles with subcloud comparison 2024-11-06T04:46:22Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:22Z- meanExpectedMeasDiff: -0.214549 delta_rms_squared: 0.817989 alpha: 0.976133 2024-11-06T04:46:22Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:22Z- meanExpectedMeasDiff: -0.872862 delta_rms_squared: 0.611198 alpha: 0.968314 2024-11-06T04:46:22Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:22Z- meanExpectedMeasDiff: 2.300012 delta_rms_squared: 0.699916 alpha: 0.972219 2024-11-06T04:46:22Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:22Z- meanExpectedMeasDiff: -0.523995 delta_rms_squared: 0.657208 alpha: 0.970467 2024-11-06T04:46:22Z- Subcloud NIS: 0.555350 2024-11-06T04:46:22Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:46:22Z- TNavPF:: sumWeights = 0.332682 2024-11-06T04:46:22Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:46:22Z- TNavPF::First Term of Map Covariance Matrix is 0.17 2024-11-06T04:46:22Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T04:46:22Z- TerrainNav::Resampling particle filter... 2024-11-06T04:46:22Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868382.27 sec, ping # 0. 2024-11-06T04:46:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:46:22Z- variances([0], [2], and psi_berg) = 0.273686 0.450531 0.000000 2024-11-06T04:46:22Z- publishEstimatesDecoder() - at 1730868382.27 sec 2024-11-06T04:46:22Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:46:22Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061632.57 594484.35 56.89 2024-11-06T04:46:22Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061632.24 594485.15 57.45 lat/lon: 36.70 -121.94 2024-11-06T04:46:22Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.27 0.45 0.31 0.00 2024-11-06T04:46:22Z- AHRS_M2 msg: 1730868382.63 epoch sec; seqNo:1529 2024-11-06T04:46:22Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.51 psi 2024-11-06T04:46:22Z- handleDepth()- Depth_Keller msg: 1730868382.64 epoch sec; seqNo:1647; depth 49.78 2024-11-06T04:46:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868382.67 epoch sec; seqNo:1647; 4061688.93 north; 594480.62 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:46:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868382.27 2024-11-06T04:46:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:22Z- waiting for 1730868386.27, time is 1730868382.67 2024-11-06T04:46:23Z- AHRS_M2 msg: 1730868383.04 epoch sec; seqNo:1530 2024-11-06T04:46:23Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.51 psi 2024-11-06T04:46:23Z- handleDepth()- Depth_Keller msg: 1730868383.04 epoch sec; seqNo:1648; depth 49.77 2024-11-06T04:46:23Z- handleDVL() - DVL msg epochMillisec: 1730868383.05 2024-11-06T04:46:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:46:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:46:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:46:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:46:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:46:23Z- handleDvl() - RDI_Pathfinder msg: 1730868383.05 sec; seqNo:602; #beams:0 2024-11-06T04:46:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:46:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.02 2024-11-06T04:46:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868383.05 epoch sec; seqNo:1648; 4061688.93 north; 594480.62 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T04:46:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868382.27 2024-11-06T04:46:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:23Z- waiting for 1730868386.27, time is 1730868383.05 2024-11-06T04:46:23Z- AHRS_M2 msg: 1730868383.44 epoch sec; seqNo:1531 2024-11-06T04:46:23Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.51 psi 2024-11-06T04:46:23Z- handleDepth()- Depth_Keller msg: 1730868383.45 epoch sec; seqNo:1649; depth 49.75 2024-11-06T04:46:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868383.46 epoch sec; seqNo:1649; 4061688.94 north; 594481.84 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:46:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868382.27 2024-11-06T04:46:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:23Z- waiting for 1730868386.27, time is 1730868383.46 2024-11-06T04:46:23Z- AHRS_M2 msg: 1730868383.85 epoch sec; seqNo:1532 2024-11-06T04:46:23Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.51 psi 2024-11-06T04:46:23Z- handleDepth()- Depth_Keller msg: 1730868383.85 epoch sec; seqNo:1650; depth 49.73 2024-11-06T04:46:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868383.87 epoch sec; seqNo:1650; 4061688.94 north; 594481.84 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T04:46:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868382.27 2024-11-06T04:46:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:24Z- waiting for 1730868386.27, time is 1730868383.87 2024-11-06T04:46:24Z- AHRS_M2 msg: 1730868384.25 epoch sec; seqNo:1533 2024-11-06T04:46:24Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.51 psi 2024-11-06T04:46:24Z- handleDepth()- Depth_Keller msg: 1730868384.26 epoch sec; seqNo:1651; depth 49.74 2024-11-06T04:46:24Z- handleDVL() - DVL msg epochMillisec: 1730868384.29 2024-11-06T04:46:24Z- handleDvl() - RDI_Pathfinder msg: 1730868384.29 sec; seqNo:603; #beams:4 2024-11-06T04:46:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.01 , 31.19 , 36.70 , 32.88 2024-11-06T04:46:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , 0.01 2024-11-06T04:46:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868384.30 epoch sec; seqNo:1651; 4061688.94 north; 594481.84 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T04:46:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868382.27 2024-11-06T04:46:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:24Z- waiting for 1730868386.27, time is 1730868384.30 2024-11-06T04:46:24Z- AHRS_M2 msg: 1730868384.65 epoch sec; seqNo:1534 2024-11-06T04:46:24Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.51 psi 2024-11-06T04:46:24Z- handleDepth()- Depth_Keller msg: 1730868384.65 epoch sec; seqNo:1652; depth 49.72 2024-11-06T04:46:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868384.67 epoch sec; seqNo:1652; 4061688.94 north; 594481.84 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:46:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868382.27 2024-11-06T04:46:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:24Z- waiting for 1730868386.27, time is 1730868384.67 2024-11-06T04:46:25Z- AHRS_M2 msg: 1730868385.06 epoch sec; seqNo:1535 2024-11-06T04:46:25Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.51 psi 2024-11-06T04:46:25Z- handleDepth()- Depth_Keller msg: 1730868385.06 epoch sec; seqNo:1653; depth 49.72 2024-11-06T04:46:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868385.07 epoch sec; seqNo:1653; 4061688.95 north; 594483.07 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:46:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868382.27 2024-11-06T04:46:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:25Z- waiting for 1730868386.27, time is 1730868385.07 2024-11-06T04:46:25Z- AHRS_M2 msg: 1730868385.46 epoch sec; seqNo:1536 2024-11-06T04:46:25Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.50 psi 2024-11-06T04:46:25Z- handleDepth()- Depth_Keller msg: 1730868385.47 epoch sec; seqNo:1654; depth 49.70 2024-11-06T04:46:25Z- handleDVL() - DVL msg epochMillisec: 1730868385.48 2024-11-06T04:46:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:46:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:46:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:46:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:46:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:46:25Z- handleDvl() - RDI_Pathfinder msg: 1730868385.48 sec; seqNo:604; #beams:0 2024-11-06T04:46:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:46:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , 0.01 2024-11-06T04:46:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868385.49 epoch sec; seqNo:1654; 4061688.95 north; 594483.07 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T04:46:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868382.27 2024-11-06T04:46:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:25Z- waiting for 1730868386.27, time is 1730868385.49 2024-11-06T04:46:25Z- AHRS_M2 msg: 1730868385.86 epoch sec; seqNo:1537 2024-11-06T04:46:25Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.50 psi 2024-11-06T04:46:25Z- handleDepth()- Depth_Keller msg: 1730868385.87 epoch sec; seqNo:1655; depth 49.69 2024-11-06T04:46:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868385.88 epoch sec; seqNo:1655; 4061688.95 north; 594483.07 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T04:46:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868382.27 2024-11-06T04:46:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:26Z- waiting for 1730868386.27, time is 1730868385.88 2024-11-06T04:46:26Z- AHRS_M2 msg: 1730868386.27 epoch sec; seqNo:1538 2024-11-06T04:46:26Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.50 psi 2024-11-06T04:46:26Z- handleDepth()- Depth_Keller msg: 1730868386.27 epoch sec; seqNo:1656; depth 49.68 2024-11-06T04:46:26Z- handleDVL() - DVL msg epochMillisec: 1730868386.29 2024-11-06T04:46:26Z- handleDvl() - RDI_Pathfinder msg: 1730868386.29 sec; seqNo:605; #beams:4 2024-11-06T04:46:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.72 , 31.60 , 36.70 , 33.13 2024-11-06T04:46:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.02 2024-11-06T04:46:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868386.32 epoch sec; seqNo:1656; 4061689.34 north; 594484.28 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T04:46:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868382.27 2024-11-06T04:46:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:26Z- waiting for 1730868386.27, time is 1730868386.32 2024-11-06T04:46:26Z- LcmTrn::updateTrn() >>>> pose time 1730868386.32, meas time 1730868386.29 <<<< 2024-11-06T04:46:26Z- LcmTrn::updateTrn() equating pose 1730868386.29 and meas times 1730868386.29 2024-11-06T04:46:26Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868386.29 2024-11-06T04:46:26Z- TerrainNav::motionUpdate() - currEstimate = (4061689, 594484.28, 49.68) 2024-11-06T04:46:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:46:26Z- TerrainNav:: SubcloudNIS is 0.56, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:46:26Z- TNavPF::Projecting Measurements 2024-11-06T04:46:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:46:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:46:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:46:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:46:26Z- TNavFilter::4 good beams. 2024-11-06T04:46:26Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:46:26Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:46:26Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:46:26Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:46:26Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:46:26Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:46:26Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:46:26Z- TerrainMapOctree: (vehN, vehE ) = (4061689.34, 594484.28). 2024-11-06T04:46:26Z- TerrainMapOctree: Min Distance = 242.01. 2024-11-06T04:46:26Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:46:26Z- Forcing Subcloud Comparison 2024-11-06T04:46:26Z- Weighting particles with subcloud comparison 2024-11-06T04:46:26Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:26Z- meanExpectedMeasDiff: 0.643072 delta_rms_squared: 0.895709 alpha: 0.978159 2024-11-06T04:46:26Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:26Z- meanExpectedMeasDiff: -0.102141 delta_rms_squared: 0.652273 alpha: 0.970250 2024-11-06T04:46:26Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:26Z- meanExpectedMeasDiff: 2.410498 delta_rms_squared: 0.764670 alpha: 0.974512 2024-11-06T04:46:26Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:26Z- meanExpectedMeasDiff: -0.939360 delta_rms_squared: 0.753801 alpha: 0.974154 2024-11-06T04:46:26Z- Subcloud NIS: 0.544750 2024-11-06T04:46:26Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:46:26Z- TNavPF:: sumWeights = 0.315363 2024-11-06T04:46:26Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:46:26Z- TNavPF::First Term of Map Covariance Matrix is 0.18 2024-11-06T04:46:26Z- TNavPF::Calculated NIS Value : 0.21 numBeams normalized NIS: 0.05 2024-11-06T04:46:26Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868386.29 sec, ping # 0. 2024-11-06T04:46:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:46:26Z- variances([0], [2], and psi_berg) = 0.278901 0.456680 0.000000 2024-11-06T04:46:26Z- publishEstimatesDecoder() - at 1730868386.29 sec 2024-11-06T04:46:26Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:46:26Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061634.67 594487.24 56.71 2024-11-06T04:46:26Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061633.04 594488.79 57.17 lat/lon: 36.70 -121.94 2024-11-06T04:46:26Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.28 0.46 0.31 0.00 2024-11-06T04:46:26Z- AHRS_M2 msg: 1730868386.67 epoch sec; seqNo:1539 2024-11-06T04:46:26Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.50 psi 2024-11-06T04:46:26Z- handleDepth()- Depth_Keller msg: 1730868386.69 epoch sec; seqNo:1657; depth 49.67 2024-11-06T04:46:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868386.71 epoch sec; seqNo:1657; 4061689.34 north; 594484.28 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:46:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868386.29 2024-11-06T04:46:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:26Z- waiting for 1730868390.29, time is 1730868386.71 2024-11-06T04:46:27Z- AHRS_M2 msg: 1730868387.08 epoch sec; seqNo:1540 2024-11-06T04:46:27Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.50 psi 2024-11-06T04:46:27Z- handleDepth()- Depth_Keller msg: 1730868387.08 epoch sec; seqNo:1658; depth 49.65 2024-11-06T04:46:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868387.12 epoch sec; seqNo:1658; 4061689.34 north; 594484.28 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:46:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868386.29 2024-11-06T04:46:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:27Z- waiting for 1730868390.29, time is 1730868387.12 2024-11-06T04:46:27Z- AHRS_M2 msg: 1730868387.48 epoch sec; seqNo:1541 2024-11-06T04:46:27Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.50 psi 2024-11-06T04:46:27Z- handleDepth()- Depth_Keller msg: 1730868387.49 epoch sec; seqNo:1659; depth 49.66 2024-11-06T04:46:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868387.49 epoch sec; seqNo:1659; 4061689.34 north; 594484.28 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:46:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868386.29 2024-11-06T04:46:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:27Z- waiting for 1730868390.29, time is 1730868387.49 2024-11-06T04:46:27Z- AHRS_M2 msg: 1730868387.88 epoch sec; seqNo:1542 2024-11-06T04:46:27Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.50 psi 2024-11-06T04:46:27Z- handleDepth()- Depth_Keller msg: 1730868387.89 epoch sec; seqNo:1660; depth 49.65 2024-11-06T04:46:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868387.90 epoch sec; seqNo:1660; 4061689.36 north; 594485.50 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:46:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868386.29 2024-11-06T04:46:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:28Z- waiting for 1730868390.29, time is 1730868387.90 2024-11-06T04:46:28Z- handleDepth()- Depth_Keller msg: 1730868388.31 epoch sec; seqNo:1661; depth 49.64 2024-11-06T04:46:28Z- handleDVL() - DVL msg epochMillisec: 1730868388.33 2024-11-06T04:46:28Z- handleDvl() - RDI_Pathfinder msg: 1730868388.33 sec; seqNo:606; #beams:4 2024-11-06T04:46:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.23 , 31.60 , 37.04 , 32.79 2024-11-06T04:46:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.02 2024-11-06T04:46:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868388.34 epoch sec; seqNo:1661; 4061689.36 north; 594485.50 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:46:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868386.29 2024-11-06T04:46:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:28Z- waiting for 1730868390.29, time is 1730868388.34 2024-11-06T04:46:28Z- AHRS_M2 msg: 1730868388.69 epoch sec; seqNo:1543 2024-11-06T04:46:28Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.49 psi 2024-11-06T04:46:28Z- handleDepth()- Depth_Keller msg: 1730868388.70 epoch sec; seqNo:1662; depth 49.61 2024-11-06T04:46:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868388.71 epoch sec; seqNo:1662; 4061689.36 north; 594485.50 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:46:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868386.29 2024-11-06T04:46:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:28Z- waiting for 1730868390.29, time is 1730868388.71 2024-11-06T04:46:29Z- AHRS_M2 msg: 1730868389.11 epoch sec; seqNo:1544 2024-11-06T04:46:29Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.49 psi 2024-11-06T04:46:29Z- handleDepth()- Depth_Keller msg: 1730868389.13 epoch sec; seqNo:1663; depth 49.61 2024-11-06T04:46:29Z- handleDVL() - DVL msg epochMillisec: 1730868389.13 2024-11-06T04:46:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:46:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:46:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:46:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:46:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:46:29Z- handleDvl() - RDI_Pathfinder msg: 1730868389.13 sec; seqNo:607; #beams:0 2024-11-06T04:46:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:46:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.02 2024-11-06T04:46:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868389.14 epoch sec; seqNo:1663; 4061689.37 north; 594486.72 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:46:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868386.29 2024-11-06T04:46:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:29Z- waiting for 1730868390.29, time is 1730868389.14 2024-11-06T04:46:29Z- AHRS_M2 msg: 1730868389.50 epoch sec; seqNo:1545 2024-11-06T04:46:29Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.49 psi 2024-11-06T04:46:29Z- handleDepth()- Depth_Keller msg: 1730868389.51 epoch sec; seqNo:1664; depth 49.61 2024-11-06T04:46:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868389.53 epoch sec; seqNo:1664; 4061689.37 north; 594486.72 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:46:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868386.29 2024-11-06T04:46:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:29Z- waiting for 1730868390.29, time is 1730868389.53 2024-11-06T04:46:29Z- AHRS_M2 msg: 1730868389.91 epoch sec; seqNo:1546 2024-11-06T04:46:29Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:46:29Z- handleDepth()- Depth_Keller msg: 1730868389.91 epoch sec; seqNo:1665; depth 49.58 2024-11-06T04:46:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868389.92 epoch sec; seqNo:1665; 4061689.75 north; 594486.72 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:46:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868386.29 2024-11-06T04:46:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:30Z- waiting for 1730868390.29, time is 1730868389.92 2024-11-06T04:46:30Z- AHRS_M2 msg: 1730868390.31 epoch sec; seqNo:1547 2024-11-06T04:46:30Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:46:30Z- handleDepth()- Depth_Keller msg: 1730868390.31 epoch sec; seqNo:1666; depth 49.57 2024-11-06T04:46:30Z- handleDVL() - DVL msg epochMillisec: 1730868390.33 2024-11-06T04:46:30Z- handleDvl() - RDI_Pathfinder msg: 1730868390.33 sec; seqNo:608; #beams:4 2024-11-06T04:46:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.23 , 32.28 , 36.87 , 32.45 2024-11-06T04:46:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , 0.01 2024-11-06T04:46:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868390.36 epoch sec; seqNo:1666; 4061689.76 north; 594487.94 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T04:46:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868386.29 2024-11-06T04:46:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:30Z- waiting for 1730868390.29, time is 1730868390.36 2024-11-06T04:46:30Z- LcmTrn::updateTrn() >>>> pose time 1730868390.36, meas time 1730868390.33 <<<< 2024-11-06T04:46:30Z- LcmTrn::updateTrn() equating pose 1730868390.33 and meas times 1730868390.33 2024-11-06T04:46:30Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868390.33 2024-11-06T04:46:30Z- TerrainNav::motionUpdate() - currEstimate = (4061690, 594487.94, 49.57) 2024-11-06T04:46:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:46:30Z- TerrainNav:: SubcloudNIS is 0.54, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:46:30Z- TNavPF::Projecting Measurements 2024-11-06T04:46:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:46:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:46:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:46:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:46:30Z- TNavFilter::4 good beams. 2024-11-06T04:46:30Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:46:30Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:46:30Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:46:30Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:46:30Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:46:30Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:46:30Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:46:30Z- TerrainMapOctree: (vehN, vehE ) = (4061689.76, 594487.94). 2024-11-06T04:46:30Z- TerrainMapOctree: Min Distance = 245.44. 2024-11-06T04:46:30Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:46:30Z- Forcing Subcloud Comparison 2024-11-06T04:46:30Z- Weighting particles with subcloud comparison 2024-11-06T04:46:30Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:30Z- meanExpectedMeasDiff: 0.511044 delta_rms_squared: 0.958329 alpha: 0.979557 2024-11-06T04:46:30Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:30Z- meanExpectedMeasDiff: 0.381254 delta_rms_squared: 0.646256 alpha: 0.969982 2024-11-06T04:46:30Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:30Z- meanExpectedMeasDiff: 2.430185 delta_rms_squared: 0.754880 alpha: 0.974190 2024-11-06T04:46:30Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:30Z- meanExpectedMeasDiff: -2.232017 delta_rms_squared: 0.765765 alpha: 0.974547 2024-11-06T04:46:30Z- Subcloud NIS: 0.532498 2024-11-06T04:46:30Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:46:30Z- TNavPF:: sumWeights = 0.166598 2024-11-06T04:46:30Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:46:30Z- TNavPF::First Term of Map Covariance Matrix is 0.11 2024-11-06T04:46:30Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.02 2024-11-06T04:46:30Z- TerrainNav::Resampling particle filter... 2024-11-06T04:46:30Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868390.33 sec, ping # 0. 2024-11-06T04:46:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:46:30Z- variances([0], [2], and psi_berg) = 0.279174 0.452843 0.000000 2024-11-06T04:46:30Z- publishEstimatesDecoder() - at 1730868390.33 sec 2024-11-06T04:46:30Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:46:30Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061633.91 594491.60 56.01 2024-11-06T04:46:30Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061633.46 594492.45 56.98 lat/lon: 36.70 -121.94 2024-11-06T04:46:30Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.28 0.45 0.31 0.00 2024-11-06T04:46:30Z- AHRS_M2 msg: 1730868390.71 epoch sec; seqNo:1548 2024-11-06T04:46:30Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:46:30Z- handleDepth()- Depth_Keller msg: 1730868390.72 epoch sec; seqNo:1667; depth 49.55 2024-11-06T04:46:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868390.76 epoch sec; seqNo:1667; 4061689.76 north; 594487.94 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T04:46:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868390.33 2024-11-06T04:46:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:30Z- waiting for 1730868394.33, time is 1730868390.76 2024-11-06T04:46:31Z- AHRS_M2 msg: 1730868391.12 epoch sec; seqNo:1549 2024-11-06T04:46:31Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:46:31Z- handleDepth()- Depth_Keller msg: 1730868391.12 epoch sec; seqNo:1668; depth 49.55 2024-11-06T04:46:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868391.13 epoch sec; seqNo:1668; 4061689.76 north; 594487.94 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T04:46:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868390.33 2024-11-06T04:46:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:31Z- waiting for 1730868394.33, time is 1730868391.13 2024-11-06T04:46:31Z- AHRS_M2 msg: 1730868391.52 epoch sec; seqNo:1550 2024-11-06T04:46:31Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:46:31Z- handleDepth()- Depth_Keller msg: 1730868391.53 epoch sec; seqNo:1669; depth 49.54 2024-11-06T04:46:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868391.54 epoch sec; seqNo:1669; 4061689.76 north; 594487.94 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T04:46:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868390.33 2024-11-06T04:46:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:31Z- waiting for 1730868394.33, time is 1730868391.54 2024-11-06T04:46:31Z- AHRS_M2 msg: 1730868391.92 epoch sec; seqNo:1551 2024-11-06T04:46:31Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:46:31Z- handleDepth()- Depth_Keller msg: 1730868391.94 epoch sec; seqNo:1670; depth 49.52 2024-11-06T04:46:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868391.95 epoch sec; seqNo:1670; 4061689.78 north; 594489.16 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:46:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868390.33 2024-11-06T04:46:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:32Z- waiting for 1730868394.33, time is 1730868391.95 2024-11-06T04:46:32Z- AHRS_M2 msg: 1730868392.33 epoch sec; seqNo:1552 2024-11-06T04:46:32Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:46:32Z- handleDepth()- Depth_Keller msg: 1730868392.34 epoch sec; seqNo:1671; depth 49.52 2024-11-06T04:46:32Z- handleDVL() - DVL msg epochMillisec: 1730868392.36 2024-11-06T04:46:32Z- handleDvl() - RDI_Pathfinder msg: 1730868392.36 sec; seqNo:609; #beams:4 2024-11-06T04:46:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.06 , 32.79 , 37.04 , 32.62 2024-11-06T04:46:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , -0.01 2024-11-06T04:46:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868392.38 epoch sec; seqNo:1671; 4061689.78 north; 594489.16 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:46:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868390.33 2024-11-06T04:46:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:32Z- waiting for 1730868394.33, time is 1730868392.38 2024-11-06T04:46:32Z- AHRS_M2 msg: 1730868392.74 epoch sec; seqNo:1553 2024-11-06T04:46:32Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:46:32Z- handleDepth()- Depth_Keller msg: 1730868392.75 epoch sec; seqNo:1672; depth 49.50 2024-11-06T04:46:32Z- handleDVL() - DVL msg epochMillisec: 1730868392.76 2024-11-06T04:46:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:46:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:46:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:46:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:46:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:46:32Z- handleDvl() - RDI_Pathfinder msg: 1730868392.76 sec; seqNo:610; #beams:0 2024-11-06T04:46:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:46:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , -0.01 2024-11-06T04:46:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868392.78 epoch sec; seqNo:1672; 4061689.78 north; 594489.16 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T04:46:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868390.33 2024-11-06T04:46:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:32Z- waiting for 1730868394.33, time is 1730868392.78 2024-11-06T04:46:33Z- AHRS_M2 msg: 1730868393.15 epoch sec; seqNo:1554 2024-11-06T04:46:33Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:46:33Z- handleDepth()- Depth_Keller msg: 1730868393.15 epoch sec; seqNo:1673; depth 49.49 2024-11-06T04:46:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868393.16 epoch sec; seqNo:1673; 4061689.78 north; 594489.16 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:46:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868390.33 2024-11-06T04:46:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:33Z- waiting for 1730868394.33, time is 1730868393.16 2024-11-06T04:46:33Z- AHRS_M2 msg: 1730868393.54 epoch sec; seqNo:1555 2024-11-06T04:46:33Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:46:33Z- handleDepth()- Depth_Keller msg: 1730868393.55 epoch sec; seqNo:1674; depth 49.47 2024-11-06T04:46:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868393.57 epoch sec; seqNo:1674; 4061690.17 north; 594490.37 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T04:46:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868390.33 2024-11-06T04:46:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:33Z- waiting for 1730868394.33, time is 1730868393.57 2024-11-06T04:46:33Z- AHRS_M2 msg: 1730868393.94 epoch sec; seqNo:1556 2024-11-06T04:46:33Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.49 psi 2024-11-06T04:46:33Z- handleDepth()- Depth_Keller msg: 1730868393.95 epoch sec; seqNo:1675; depth 49.46 2024-11-06T04:46:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868393.96 epoch sec; seqNo:1675; 4061690.17 north; 594490.37 east; 36.70 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T04:46:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868390.33 2024-11-06T04:46:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:34Z- waiting for 1730868394.33, time is 1730868393.96 2024-11-06T04:46:34Z- AHRS_M2 msg: 1730868394.35 epoch sec; seqNo:1557 2024-11-06T04:46:34Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.49 psi 2024-11-06T04:46:34Z- handleDepth()- Depth_Keller msg: 1730868394.36 epoch sec; seqNo:1676; depth 49.46 2024-11-06T04:46:34Z- handleDVL() - DVL msg epochMillisec: 1730868394.38 2024-11-06T04:46:34Z- handleDvl() - RDI_Pathfinder msg: 1730868394.38 sec; seqNo:611; #beams:4 2024-11-06T04:46:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.06 , 32.62 , 36.70 , 32.45 2024-11-06T04:46:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , -0.01 2024-11-06T04:46:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868394.40 epoch sec; seqNo:1676; 4061690.17 north; 594490.37 east; 36.70 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T04:46:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868390.33 2024-11-06T04:46:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:34Z- waiting for 1730868394.33, time is 1730868394.40 2024-11-06T04:46:34Z- LcmTrn::updateTrn() >>>> pose time 1730868394.40, meas time 1730868394.38 <<<< 2024-11-06T04:46:34Z- LcmTrn::updateTrn() equating pose 1730868394.38 and meas times 1730868394.38 2024-11-06T04:46:34Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868394.38 2024-11-06T04:46:34Z- TerrainNav::motionUpdate() - currEstimate = (4061690, 594490.37, 49.46) 2024-11-06T04:46:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:46:34Z- TerrainNav:: SubcloudNIS is 0.53, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:46:34Z- TNavPF::Projecting Measurements 2024-11-06T04:46:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:46:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:46:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:46:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:46:34Z- TNavFilter::4 good beams. 2024-11-06T04:46:34Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:46:34Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:46:34Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:46:34Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:46:34Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:46:34Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:46:34Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:46:34Z- TerrainMapOctree: (vehN, vehE ) = (4061690.17, 594490.37). 2024-11-06T04:46:34Z- TerrainMapOctree: Min Distance = 247.71. 2024-11-06T04:46:34Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:46:34Z- Forcing Subcloud Comparison 2024-11-06T04:46:34Z- Weighting particles with subcloud comparison 2024-11-06T04:46:34Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:34Z- meanExpectedMeasDiff: -0.092845 delta_rms_squared: 0.880873 alpha: 0.977799 2024-11-06T04:46:34Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:34Z- meanExpectedMeasDiff: 0.607788 delta_rms_squared: 0.616455 alpha: 0.968576 2024-11-06T04:46:34Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:34Z- meanExpectedMeasDiff: 2.213868 delta_rms_squared: 0.716931 alpha: 0.972860 2024-11-06T04:46:34Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:34Z- meanExpectedMeasDiff: -2.697147 delta_rms_squared: 0.745151 alpha: 0.973861 2024-11-06T04:46:34Z- Subcloud NIS: 0.518007 2024-11-06T04:46:34Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:46:34Z- TNavPF:: sumWeights = 0.136354 2024-11-06T04:46:34Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:46:34Z- TNavPF::First Term of Map Covariance Matrix is 0.07 2024-11-06T04:46:34Z- TNavPF::Calculated NIS Value : 0.07 numBeams normalized NIS: 0.02 2024-11-06T04:46:34Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868394.38 sec, ping # 0. 2024-11-06T04:46:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:46:34Z- variances([0], [2], and psi_berg) = 0.282003 0.453408 0.000000 2024-11-06T04:46:34Z- publishEstimatesDecoder() - at 1730868394.38 sec 2024-11-06T04:46:34Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:46:34Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061633.20 594494.74 56.95 2024-11-06T04:46:34Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061633.86 594494.89 56.91 lat/lon: 36.70 -121.94 2024-11-06T04:46:34Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.28 0.45 0.30 0.00 2024-11-06T04:46:34Z- AHRS_M2 msg: 1730868394.76 epoch sec; seqNo:1558 2024-11-06T04:46:34Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.49 psi 2024-11-06T04:46:34Z- handleDepth()- Depth_Keller msg: 1730868394.77 epoch sec; seqNo:1677; depth 49.46 2024-11-06T04:46:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868394.79 epoch sec; seqNo:1677; 4061690.18 north; 594491.59 east; 36.70 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T04:46:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868394.38 2024-11-06T04:46:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:34Z- waiting for 1730868398.38, time is 1730868394.79 2024-11-06T04:46:35Z- AHRS_M2 msg: 1730868395.15 epoch sec; seqNo:1559 2024-11-06T04:46:35Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.49 psi 2024-11-06T04:46:35Z- handleDepth()- Depth_Keller msg: 1730868395.16 epoch sec; seqNo:1678; depth 49.44 2024-11-06T04:46:35Z- handleDVL() - DVL msg epochMillisec: 1730868395.16 2024-11-06T04:46:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:46:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:46:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:46:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:46:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:46:35Z- handleDvl() - RDI_Pathfinder msg: 1730868395.16 sec; seqNo:612; #beams:0 2024-11-06T04:46:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:46:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , -0.01 2024-11-06T04:46:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868395.17 epoch sec; seqNo:1678; 4061690.18 north; 594491.59 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T04:46:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868394.38 2024-11-06T04:46:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:35Z- waiting for 1730868398.38, time is 1730868395.17 2024-11-06T04:46:35Z- AHRS_M2 msg: 1730868395.56 epoch sec; seqNo:1560 2024-11-06T04:46:35Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.49 psi 2024-11-06T04:46:35Z- handleDepth()- Depth_Keller msg: 1730868395.57 epoch sec; seqNo:1679; depth 49.43 2024-11-06T04:46:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868395.58 epoch sec; seqNo:1679; 4061690.18 north; 594491.59 east; 36.70 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T04:46:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868394.38 2024-11-06T04:46:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:35Z- waiting for 1730868398.38, time is 1730868395.58 2024-11-06T04:46:35Z- handleDepth()- Depth_Keller msg: 1730868395.98 epoch sec; seqNo:1680; depth 49.43 2024-11-06T04:46:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868396.00 epoch sec; seqNo:1680; 4061690.18 north; 594491.59 east; 36.70 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T04:46:36Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:46:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868394.38 2024-11-06T04:46:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:36Z- waiting for 1730868398.38, time is 1730868396.00 2024-11-06T04:46:36Z- AHRS_M2 msg: 1730868396.37 epoch sec; seqNo:1561 2024-11-06T04:46:36Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.49 psi 2024-11-06T04:46:36Z- handleDepth()- Depth_Keller msg: 1730868396.38 epoch sec; seqNo:1681; depth 49.44 2024-11-06T04:46:36Z- handleDVL() - DVL msg epochMillisec: 1730868396.40 2024-11-06T04:46:36Z- handleDvl() - RDI_Pathfinder msg: 1730868396.40 sec; seqNo:613; #beams:4 2024-11-06T04:46:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.74 , 32.45 , 36.70 , 32.79 2024-11-06T04:46:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , 0.00 2024-11-06T04:46:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868396.42 epoch sec; seqNo:1681; 4061690.20 north; 594492.82 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T04:46:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868394.38 2024-11-06T04:46:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:36Z- waiting for 1730868398.38, time is 1730868396.42 2024-11-06T04:46:36Z- handleDepth()- Depth_Keller msg: 1730868396.79 epoch sec; seqNo:1682; depth 49.43 2024-11-06T04:46:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868396.83 epoch sec; seqNo:1682; 4061690.58 north; 594492.81 east; 36.70 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T04:46:36Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:46:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868394.38 2024-11-06T04:46:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:36Z- waiting for 1730868398.38, time is 1730868396.83 2024-11-06T04:46:37Z- AHRS_M2 msg: 1730868397.18 epoch sec; seqNo:1562 2024-11-06T04:46:37Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.49 psi 2024-11-06T04:46:37Z- handleDepth()- Depth_Keller msg: 1730868397.19 epoch sec; seqNo:1683; depth 49.42 2024-11-06T04:46:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868397.20 epoch sec; seqNo:1683; 4061690.58 north; 594492.81 east; 36.70 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T04:46:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868394.38 2024-11-06T04:46:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:37Z- waiting for 1730868398.38, time is 1730868397.20 2024-11-06T04:46:37Z- AHRS_M2 msg: 1730868397.58 epoch sec; seqNo:1563 2024-11-06T04:46:37Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.50 psi 2024-11-06T04:46:37Z- handleDepth()- Depth_Keller msg: 1730868397.59 epoch sec; seqNo:1684; depth 49.42 2024-11-06T04:46:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868397.61 epoch sec; seqNo:1684; 4061690.58 north; 594492.81 east; 36.70 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T04:46:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868394.38 2024-11-06T04:46:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:37Z- waiting for 1730868398.38, time is 1730868397.61 2024-11-06T04:46:37Z- AHRS_M2 msg: 1730868397.98 epoch sec; seqNo:1564 2024-11-06T04:46:37Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.50 psi 2024-11-06T04:46:37Z- handleDepth()- Depth_Keller msg: 1730868397.99 epoch sec; seqNo:1685; depth 49.42 2024-11-06T04:46:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868398.00 epoch sec; seqNo:1685; 4061690.59 north; 594494.03 east; 36.70 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T04:46:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868394.38 2024-11-06T04:46:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:38Z- waiting for 1730868398.38, time is 1730868398.00 2024-11-06T04:46:38Z- AHRS_M2 msg: 1730868398.40 epoch sec; seqNo:1565 2024-11-06T04:46:38Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.50 psi 2024-11-06T04:46:38Z- handleDepth()- Depth_Keller msg: 1730868398.41 epoch sec; seqNo:1686; depth 49.44 2024-11-06T04:46:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868398.44 epoch sec; seqNo:1686; 4061690.59 north; 594494.03 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T04:46:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868394.38 2024-11-06T04:46:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:38Z- waiting for 1730868398.38, time is 1730868398.44 2024-11-06T04:46:38Z- LcmTrn::updateTrn() >>>> pose time 1730868398.44, meas time 1730868396.40 <<<< 2024-11-06T04:46:38Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868398.44 2024-11-06T04:46:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868396.40 sec, ping # 0; waiting for INS data more recent than 1730868394.38... 2024-11-06T04:46:38Z- TerrainNav::motionUpdate() - currEstimate = (4061691, 594494.03, 49.44) 2024-11-06T04:46:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:46:38Z- TerrainNav:: SubcloudNIS is 0.52, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:46:38Z- TNavPF::Projecting Measurements 2024-11-06T04:46:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:46:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:46:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:46:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:46:38Z- TNavFilter::4 good beams. 2024-11-06T04:46:38Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:46:38Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:46:38Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:46:38Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:46:38Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:46:38Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:46:38Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:46:38Z- TerrainMapOctree: (vehN, vehE ) = (4061690.38, 594492.20). 2024-11-06T04:46:38Z- TerrainMapOctree: Min Distance = 249.43. 2024-11-06T04:46:38Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:46:38Z- Forcing Subcloud Comparison 2024-11-06T04:46:38Z- Weighting particles with subcloud comparison 2024-11-06T04:46:38Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:38Z- meanExpectedMeasDiff: 0.509331 delta_rms_squared: 0.899010 alpha: 0.978237 2024-11-06T04:46:38Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:38Z- meanExpectedMeasDiff: 0.484602 delta_rms_squared: 0.625631 alpha: 0.969023 2024-11-06T04:46:38Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:38Z- meanExpectedMeasDiff: 2.299401 delta_rms_squared: 0.726075 alpha: 0.973193 2024-11-06T04:46:38Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:38Z- meanExpectedMeasDiff: -2.455273 delta_rms_squared: 0.762754 alpha: 0.974449 2024-11-06T04:46:38Z- Subcloud NIS: 0.498742 2024-11-06T04:46:38Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:46:38Z- TNavPF:: sumWeights = 0.154464 2024-11-06T04:46:38Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:46:38Z- TNavPF::First Term of Map Covariance Matrix is 0.10 2024-11-06T04:46:38Z- TNavPF::Calculated NIS Value : 0.09 numBeams normalized NIS: 0.02 2024-11-06T04:46:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868396.40 sec. 2024-11-06T04:46:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:46:38Z- variances([0], [2], and psi_berg) = 0.286376 0.453977 0.000000 2024-11-06T04:46:38Z- publishEstimatesDecoder() - at 1730868398.44 sec 2024-11-06T04:46:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:46:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061634.19 594497.49 56.89 2024-11-06T04:46:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061634.28 594498.55 56.83 lat/lon: 36.70 -121.94 2024-11-06T04:46:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.29 0.45 0.56 0.00 2024-11-06T04:46:38Z- AHRS_M2 msg: 1730868398.80 epoch sec; seqNo:1566 2024-11-06T04:46:38Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.50 psi 2024-11-06T04:46:38Z- handleDepth()- Depth_Keller msg: 1730868398.80 epoch sec; seqNo:1687; depth 49.43 2024-11-06T04:46:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868398.84 epoch sec; seqNo:1687; 4061690.59 north; 594494.03 east; 36.70 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T04:46:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868398.44 2024-11-06T04:46:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:38Z- waiting for 1730868402.44, time is 1730868398.84 2024-11-06T04:46:39Z- AHRS_M2 msg: 1730868399.20 epoch sec; seqNo:1567 2024-11-06T04:46:39Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.50 psi 2024-11-06T04:46:39Z- handleDepth()- Depth_Keller msg: 1730868399.20 epoch sec; seqNo:1688; depth 49.43 2024-11-06T04:46:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868399.21 epoch sec; seqNo:1688; 4061690.59 north; 594494.03 east; 36.70 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T04:46:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868398.44 2024-11-06T04:46:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:39Z- waiting for 1730868402.44, time is 1730868399.21 2024-11-06T04:46:39Z- AHRS_M2 msg: 1730868399.60 epoch sec; seqNo:1568 2024-11-06T04:46:39Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.50 psi 2024-11-06T04:46:39Z- handleDepth()- Depth_Keller msg: 1730868399.61 epoch sec; seqNo:1689; depth 49.44 2024-11-06T04:46:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868399.62 epoch sec; seqNo:1689; 4061690.98 north; 594495.25 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T04:46:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868398.44 2024-11-06T04:46:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:39Z- waiting for 1730868402.44, time is 1730868399.62 2024-11-06T04:46:40Z- AHRS_M2 msg: 1730868400.00 epoch sec; seqNo:1569 2024-11-06T04:46:40Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.50 psi 2024-11-06T04:46:40Z- handleDepth()- Depth_Keller msg: 1730868400.02 epoch sec; seqNo:1690; depth 49.45 2024-11-06T04:46:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868400.03 epoch sec; seqNo:1690; 4061690.98 north; 594495.25 east; 36.70 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T04:46:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868398.44 2024-11-06T04:46:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:40Z- waiting for 1730868402.44, time is 1730868400.03 2024-11-06T04:46:40Z- AHRS_M2 msg: 1730868400.41 epoch sec; seqNo:1570 2024-11-06T04:46:40Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T04:46:40Z- handleDepth()- Depth_Keller msg: 1730868400.41 epoch sec; seqNo:1691; depth 49.45 2024-11-06T04:46:40Z- handleDVL() - DVL msg epochMillisec: 1730868400.43 2024-11-06T04:46:40Z- handleDvl() - RDI_Pathfinder msg: 1730868400.43 sec; seqNo:614; #beams:4 2024-11-06T04:46:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.25 , 32.45 , 36.36 , 32.96 2024-11-06T04:46:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , 0.04 2024-11-06T04:46:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868400.46 epoch sec; seqNo:1691; 4061690.98 north; 594495.25 east; 36.70 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T04:46:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868398.44 2024-11-06T04:46:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:40Z- waiting for 1730868402.44, time is 1730868400.46 2024-11-06T04:46:40Z- AHRS_M2 msg: 1730868400.82 epoch sec; seqNo:1571 2024-11-06T04:46:40Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.50 psi 2024-11-06T04:46:40Z- handleDepth()- Depth_Keller msg: 1730868400.83 epoch sec; seqNo:1692; depth 49.44 2024-11-06T04:46:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868400.84 epoch sec; seqNo:1692; 4061690.99 north; 594496.47 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T04:46:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868398.44 2024-11-06T04:46:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:40Z- waiting for 1730868402.44, time is 1730868400.84 2024-11-06T04:46:41Z- AHRS_M2 msg: 1730868401.21 epoch sec; seqNo:1572 2024-11-06T04:46:41Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.50 psi 2024-11-06T04:46:41Z- handleDepth()- Depth_Keller msg: 1730868401.22 epoch sec; seqNo:1693; depth 49.47 2024-11-06T04:46:41Z- handleDVL() - DVL msg epochMillisec: 1730868401.23 2024-11-06T04:46:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:46:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:46:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:46:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:46:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:46:41Z- handleDvl() - RDI_Pathfinder msg: 1730868401.23 sec; seqNo:615; #beams:0 2024-11-06T04:46:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:46:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , 0.04 2024-11-06T04:46:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868401.24 epoch sec; seqNo:1693; 4061690.99 north; 594496.47 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T04:46:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868398.44 2024-11-06T04:46:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:41Z- waiting for 1730868402.44, time is 1730868401.24 2024-11-06T04:46:41Z- AHRS_M2 msg: 1730868401.62 epoch sec; seqNo:1573 2024-11-06T04:46:41Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T04:46:41Z- handleDepth()- Depth_Keller msg: 1730868401.62 epoch sec; seqNo:1694; depth 49.48 2024-11-06T04:46:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868401.63 epoch sec; seqNo:1694; 4061690.99 north; 594496.47 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T04:46:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868398.44 2024-11-06T04:46:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:41Z- waiting for 1730868402.44, time is 1730868401.63 2024-11-06T04:46:42Z- AHRS_M2 msg: 1730868402.02 epoch sec; seqNo:1574 2024-11-06T04:46:42Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T04:46:42Z- handleDepth()- Depth_Keller msg: 1730868402.03 epoch sec; seqNo:1695; depth 49.48 2024-11-06T04:46:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868402.04 epoch sec; seqNo:1695; 4061690.99 north; 594496.47 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T04:46:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868398.44 2024-11-06T04:46:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:42Z- waiting for 1730868402.44, time is 1730868402.04 2024-11-06T04:46:42Z- AHRS_M2 msg: 1730868402.43 epoch sec; seqNo:1575 2024-11-06T04:46:42Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T04:46:42Z- handleDepth()- Depth_Keller msg: 1730868402.44 epoch sec; seqNo:1696; depth 49.47 2024-11-06T04:46:42Z- handleDVL() - DVL msg epochMillisec: 1730868402.46 2024-11-06T04:46:42Z- handleDvl() - RDI_Pathfinder msg: 1730868402.46 sec; seqNo:616; #beams:4 2024-11-06T04:46:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.44 , 32.28 , 36.36 , 33.13 2024-11-06T04:46:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.06 2024-11-06T04:46:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868402.47 epoch sec; seqNo:1696; 4061691.01 north; 594497.69 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T04:46:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868398.44 2024-11-06T04:46:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:42Z- waiting for 1730868402.44, time is 1730868402.47 2024-11-06T04:46:42Z- LcmTrn::updateTrn() >>>> pose time 1730868402.47, meas time 1730868402.46 <<<< 2024-11-06T04:46:42Z- LcmTrn::updateTrn() equating pose 1730868402.46 and meas times 1730868402.46 2024-11-06T04:46:42Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868402.46 2024-11-06T04:46:42Z- TerrainNav::motionUpdate() - currEstimate = (4061691, 594497.69, 49.47) 2024-11-06T04:46:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:46:42Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:46:42Z- TNavPF::Projecting Measurements 2024-11-06T04:46:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:46:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:46:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:46:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:46:42Z- TNavFilter::4 good beams. 2024-11-06T04:46:42Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:46:42Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:46:42Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:46:42Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:46:42Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:46:42Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:46:42Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:46:42Z- TerrainMapOctree: (vehN, vehE ) = (4061691.01, 594497.69). 2024-11-06T04:46:42Z- TerrainMapOctree: Min Distance = 254.62. 2024-11-06T04:46:42Z- TerrainMapOctree: Using tile 4. 2024-11-06T04:46:42Z- Forcing Subcloud Comparison 2024-11-06T04:46:42Z- Weighting particles with subcloud comparison 2024-11-06T04:46:42Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:42Z- meanExpectedMeasDiff: 2.129553 delta_rms_squared: 1.638423 alpha: 0.987940 2024-11-06T04:46:42Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:42Z- meanExpectedMeasDiff: 0.229146 delta_rms_squared: 0.921716 alpha: 0.978762 2024-11-06T04:46:42Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:42Z- meanExpectedMeasDiff: 2.258094 delta_rms_squared: 1.044648 alpha: 0.981214 2024-11-06T04:46:42Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:42Z- meanExpectedMeasDiff: -2.531895 delta_rms_squared: 1.143474 alpha: 0.982810 2024-11-06T04:46:42Z- Subcloud NIS: 0.499466 2024-11-06T04:46:42Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:46:42Z- TNavPF:: sumWeights = 0.087533 2024-11-06T04:46:42Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:46:42Z- TNavPF::First Term of Map Covariance Matrix is 0.33 2024-11-06T04:46:42Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T04:46:42Z- TerrainNav::Resampling particle filter... 2024-11-06T04:46:42Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868402.46 sec, ping # 0. 2024-11-06T04:46:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:46:42Z- variances([0], [2], and psi_berg) = 0.296686 0.467929 0.000000 2024-11-06T04:46:42Z- publishEstimatesDecoder() - at 1730868402.46 sec 2024-11-06T04:46:42Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:46:42Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061635.11 594501.37 56.26 2024-11-06T04:46:42Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061634.72 594502.19 56.65 lat/lon: 36.70 -121.94 2024-11-06T04:46:42Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.30 0.47 0.36 0.00 2024-11-06T04:46:42Z- AHRS_M2 msg: 1730868402.86 epoch sec; seqNo:1576 2024-11-06T04:46:42Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T04:46:42Z- handleDepth()- Depth_Keller msg: 1730868402.87 epoch sec; seqNo:1697; depth 49.47 2024-11-06T04:46:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868402.91 epoch sec; seqNo:1697; 4061691.39 north; 594497.68 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T04:46:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868402.46 2024-11-06T04:46:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:43Z- waiting for 1730868406.46, time is 1730868402.91 2024-11-06T04:46:43Z- AHRS_M2 msg: 1730868403.24 epoch sec; seqNo:1577 2024-11-06T04:46:43Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T04:46:43Z- handleDepth()- Depth_Keller msg: 1730868403.24 epoch sec; seqNo:1698; depth 49.49 2024-11-06T04:46:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868403.25 epoch sec; seqNo:1698; 4061691.39 north; 594497.68 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:46:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868402.46 2024-11-06T04:46:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:43Z- waiting for 1730868406.46, time is 1730868403.25 2024-11-06T04:46:43Z- AHRS_M2 msg: 1730868403.64 epoch sec; seqNo:1578 2024-11-06T04:46:43Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T04:46:43Z- handleDepth()- Depth_Keller msg: 1730868403.65 epoch sec; seqNo:1699; depth 49.50 2024-11-06T04:46:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868403.66 epoch sec; seqNo:1699; 4061691.39 north; 594497.68 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T04:46:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868402.46 2024-11-06T04:46:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:43Z- waiting for 1730868406.46, time is 1730868403.66 2024-11-06T04:46:44Z- AHRS_M2 msg: 1730868404.05 epoch sec; seqNo:1579 2024-11-06T04:46:44Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:46:44Z- handleDepth()- Depth_Keller msg: 1730868404.05 epoch sec; seqNo:1700; depth 49.48 2024-11-06T04:46:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868404.08 epoch sec; seqNo:1700; 4061691.40 north; 594498.90 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T04:46:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868402.46 2024-11-06T04:46:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:44Z- waiting for 1730868406.46, time is 1730868404.08 2024-11-06T04:46:44Z- AHRS_M2 msg: 1730868404.45 epoch sec; seqNo:1580 2024-11-06T04:46:44Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:46:44Z- handleDepth()- Depth_Keller msg: 1730868404.45 epoch sec; seqNo:1701; depth 49.48 2024-11-06T04:46:44Z- handleDVL() - DVL msg epochMillisec: 1730868404.47 2024-11-06T04:46:44Z- handleDvl() - RDI_Pathfinder msg: 1730868404.47 sec; seqNo:617; #beams:4 2024-11-06T04:46:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.93 , 32.62 , 36.36 , 33.13 2024-11-06T04:46:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , 0.03 2024-11-06T04:46:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868404.50 epoch sec; seqNo:1701; 4061691.40 north; 594498.90 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T04:46:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868402.46 2024-11-06T04:46:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:44Z- waiting for 1730868406.46, time is 1730868404.50 2024-11-06T04:46:44Z- AHRS_M2 msg: 1730868404.85 epoch sec; seqNo:1581 2024-11-06T04:46:44Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:46:44Z- handleDepth()- Depth_Keller msg: 1730868404.86 epoch sec; seqNo:1702; depth 49.49 2024-11-06T04:46:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868404.87 epoch sec; seqNo:1702; 4061691.40 north; 594498.90 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:46:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868402.46 2024-11-06T04:46:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:45Z- waiting for 1730868406.46, time is 1730868404.87 2024-11-06T04:46:45Z- AHRS_M2 msg: 1730868405.26 epoch sec; seqNo:1582 2024-11-06T04:46:45Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:46:45Z- handleDepth()- Depth_Keller msg: 1730868405.28 epoch sec; seqNo:1703; depth 49.49 2024-11-06T04:46:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868405.29 epoch sec; seqNo:1703; 4061691.41 north; 594500.12 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:46:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868402.46 2024-11-06T04:46:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:45Z- waiting for 1730868406.46, time is 1730868405.29 2024-11-06T04:46:45Z- AHRS_M2 msg: 1730868405.66 epoch sec; seqNo:1583 2024-11-06T04:46:45Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:46:45Z- handleDepth()- Depth_Keller msg: 1730868405.67 epoch sec; seqNo:1704; depth 49.49 2024-11-06T04:46:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868405.68 epoch sec; seqNo:1704; 4061691.41 north; 594500.12 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:46:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868402.46 2024-11-06T04:46:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:45Z- waiting for 1730868406.46, time is 1730868405.68 2024-11-06T04:46:46Z- AHRS_M2 msg: 1730868406.07 epoch sec; seqNo:1584 2024-11-06T04:46:46Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.52 psi 2024-11-06T04:46:46Z- handleDepth()- Depth_Keller msg: 1730868406.07 epoch sec; seqNo:1705; depth 49.48 2024-11-06T04:46:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868406.08 epoch sec; seqNo:1705; 4061691.41 north; 594500.12 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T04:46:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868402.46 2024-11-06T04:46:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:46Z- waiting for 1730868406.46, time is 1730868406.08 2024-11-06T04:46:46Z- AHRS_M2 msg: 1730868406.47 epoch sec; seqNo:1585 2024-11-06T04:46:46Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.52 psi 2024-11-06T04:46:46Z- handleDepth()- Depth_Keller msg: 1730868406.48 epoch sec; seqNo:1706; depth 49.49 2024-11-06T04:46:46Z- handleDVL() - DVL msg epochMillisec: 1730868406.50 2024-11-06T04:46:46Z- handleDvl() - RDI_Pathfinder msg: 1730868406.50 sec; seqNo:618; #beams:4 2024-11-06T04:46:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.10 , 32.62 , 36.36 , 33.13 2024-11-06T04:46:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , -0.04 2024-11-06T04:46:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868406.51 epoch sec; seqNo:1706; 4061691.43 north; 594501.34 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:46:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868402.46 2024-11-06T04:46:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:46Z- waiting for 1730868406.46, time is 1730868406.51 2024-11-06T04:46:46Z- LcmTrn::updateTrn() >>>> pose time 1730868406.51, meas time 1730868406.50 <<<< 2024-11-06T04:46:46Z- LcmTrn::updateTrn() equating pose 1730868406.50 and meas times 1730868406.50 2024-11-06T04:46:46Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868406.50 2024-11-06T04:46:46Z- TerrainNav::motionUpdate() - currEstimate = (4061691, 594501.34, 49.49) 2024-11-06T04:46:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:46:46Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:46:46Z- TNavPF::Projecting Measurements 2024-11-06T04:46:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:46:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:46:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:46:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:46:46Z- TNavFilter::4 good beams. 2024-11-06T04:46:46Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:46:46Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:46:46Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:46:46Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:46:46Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:46:46Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:46:46Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:46:46Z- TerrainMapOctree: (vehN, vehE ) = (4061691.43, 594501.34). 2024-11-06T04:46:46Z- TerrainMapOctree: Min Distance = 255.46. 2024-11-06T04:46:46Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:46:46Z- TerrainMapOctree: Switching to tile 5. 2024-11-06T04:46:48Z- TerrainMapOctree::Octree tile load /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile5.bo took 1.243527 seconds. 2024-11-06T04:46:48Z- Forcing Subcloud Comparison 2024-11-06T04:46:48Z- Weighting particles with subcloud comparison 2024-11-06T04:46:48Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:48Z- meanExpectedMeasDiff: 0.328254 delta_rms_squared: 1.055856 alpha: 0.981410 2024-11-06T04:46:48Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:48Z- meanExpectedMeasDiff: 0.382134 delta_rms_squared: 0.702675 alpha: 0.972325 2024-11-06T04:46:48Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:48Z- meanExpectedMeasDiff: 2.199518 delta_rms_squared: 0.790116 alpha: 0.975312 2024-11-06T04:46:48Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:48Z- meanExpectedMeasDiff: -2.945856 delta_rms_squared: 0.882455 alpha: 0.977838 2024-11-06T04:46:48Z- Subcloud NIS: 0.504456 2024-11-06T04:46:48Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:46:48Z- TNavPF:: sumWeights = 0.113011 2024-11-06T04:46:48Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:46:48Z- TNavPF::First Term of Map Covariance Matrix is 0.08 2024-11-06T04:46:48Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.01 2024-11-06T04:46:48Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868406.50 sec, ping # 0. 2024-11-06T04:46:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:46:48Z- variances([0], [2], and psi_berg) = 0.295137 0.465785 0.000000 2024-11-06T04:46:48Z- publishEstimatesDecoder() - at 1730868406.50 sec 2024-11-06T04:46:48Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:46:48Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061634.29 594506.61 56.78 2024-11-06T04:46:48Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061635.15 594505.84 56.72 lat/lon: 36.70 -121.94 2024-11-06T04:46:48Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.30 0.47 0.33 0.00 2024-11-06T04:46:48Z- AHRS_M2 msg: 1730868406.88 epoch sec; seqNo:1586 2024-11-06T04:46:48Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.52 psi 2024-11-06T04:46:48Z- handleDepth()- Depth_Keller msg: 1730868406.88 epoch sec; seqNo:1707; depth 49.47 2024-11-06T04:46:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868406.89 epoch sec; seqNo:1707; 4061691.43 north; 594501.34 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T04:46:48Z- AHRS_M2 msg: 1730868407.28 epoch sec; seqNo:1587 2024-11-06T04:46:48Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.52 psi 2024-11-06T04:46:48Z- handleDepth()- Depth_Keller msg: 1730868407.28 epoch sec; seqNo:1708; depth 49.48 2024-11-06T04:46:48Z- handleDVL() - DVL msg epochMillisec: 1730868407.28 2024-11-06T04:46:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:46:48Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:46:48Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:46:48Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:46:48Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:46:48Z- handleDvl() - RDI_Pathfinder msg: 1730868407.28 sec; seqNo:619; #beams:0 2024-11-06T04:46:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:46:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , -0.04 2024-11-06T04:46:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868407.29 epoch sec; seqNo:1708; 4061691.43 north; 594501.34 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T04:46:48Z- AHRS_M2 msg: 1730868407.68 epoch sec; seqNo:1588 2024-11-06T04:46:48Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:46:48Z- handleDepth()- Depth_Keller msg: 1730868407.69 epoch sec; seqNo:1709; depth 49.47 2024-11-06T04:46:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868407.70 epoch sec; seqNo:1709; 4061691.43 north; 594501.34 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T04:46:48Z- AHRS_M2 msg: 1730868408.08 epoch sec; seqNo:1589 2024-11-06T04:46:48Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:46:48Z- handleDepth()- Depth_Keller msg: 1730868408.11 epoch sec; seqNo:1710; depth 49.47 2024-11-06T04:46:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868408.15 epoch sec; seqNo:1710; 4061691.44 north; 594502.57 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T04:46:48Z- AHRS_M2 msg: 1730868408.49 epoch sec; seqNo:1590 2024-11-06T04:46:48Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.53 psi 2024-11-06T04:46:48Z- 13 msgs in 200 ms and 14 handle calls 2024-11-06T04:46:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868406.50 2024-11-06T04:46:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:48Z- waiting for 1730868410.50, time is 1730868408.49 2024-11-06T04:46:48Z- handleDepth()- Depth_Keller msg: 1730868408.50 epoch sec; seqNo:1711; depth 49.47 2024-11-06T04:46:48Z- handleDVL() - DVL msg epochMillisec: 1730868408.53 2024-11-06T04:46:48Z- handleDvl() - RDI_Pathfinder msg: 1730868408.53 sec; seqNo:620; #beams:4 2024-11-06T04:46:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.27 , 32.45 , 36.19 , 32.96 2024-11-06T04:46:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , -0.06 2024-11-06T04:46:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868408.55 epoch sec; seqNo:1711; 4061691.82 north; 594502.56 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T04:46:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868406.50 2024-11-06T04:46:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:48Z- waiting for 1730868410.50, time is 1730868408.55 2024-11-06T04:46:48Z- AHRS_M2 msg: 1730868408.89 epoch sec; seqNo:1591 2024-11-06T04:46:48Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:46:48Z- handleDepth()- Depth_Keller msg: 1730868408.90 epoch sec; seqNo:1712; depth 49.47 2024-11-06T04:46:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868408.91 epoch sec; seqNo:1712; 4061691.82 north; 594502.56 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T04:46:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868406.50 2024-11-06T04:46:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:49Z- waiting for 1730868410.50, time is 1730868408.91 2024-11-06T04:46:49Z- AHRS_M2 msg: 1730868409.30 epoch sec; seqNo:1592 2024-11-06T04:46:49Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:46:49Z- handleDepth()- Depth_Keller msg: 1730868409.30 epoch sec; seqNo:1713; depth 49.48 2024-11-06T04:46:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868409.31 epoch sec; seqNo:1713; 4061691.83 north; 594503.78 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T04:46:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868406.50 2024-11-06T04:46:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:49Z- waiting for 1730868410.50, time is 1730868409.31 2024-11-06T04:46:49Z- AHRS_M2 msg: 1730868409.70 epoch sec; seqNo:1593 2024-11-06T04:46:49Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:46:49Z- handleDepth()- Depth_Keller msg: 1730868409.70 epoch sec; seqNo:1714; depth 49.47 2024-11-06T04:46:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868409.72 epoch sec; seqNo:1714; 4061691.83 north; 594503.78 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T04:46:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868406.50 2024-11-06T04:46:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:49Z- waiting for 1730868410.50, time is 1730868409.72 2024-11-06T04:46:50Z- AHRS_M2 msg: 1730868410.14 epoch sec; seqNo:1594 2024-11-06T04:46:50Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:46:50Z- handleDepth()- Depth_Keller msg: 1730868410.15 epoch sec; seqNo:1715; depth 49.48 2024-11-06T04:46:50Z- handleDVL() - DVL msg epochMillisec: 1730868410.17 2024-11-06T04:46:50Z- handleDvl() - RDI_Pathfinder msg: 1730868410.17 sec; seqNo:621; #beams:4 2024-11-06T04:46:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.44 , 32.45 , 35.85 , 32.96 2024-11-06T04:46:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.08 , -0.02 2024-11-06T04:46:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868410.21 epoch sec; seqNo:1715; 4061691.83 north; 594503.78 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T04:46:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868406.50 2024-11-06T04:46:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:50Z- waiting for 1730868410.50, time is 1730868410.21 2024-11-06T04:46:50Z- AHRS_M2 msg: 1730868410.51 epoch sec; seqNo:1595 2024-11-06T04:46:50Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.52 psi 2024-11-06T04:46:50Z- handleDepth()- Depth_Keller msg: 1730868410.51 epoch sec; seqNo:1716; depth 49.49 2024-11-06T04:46:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868410.53 epoch sec; seqNo:1716; 4061691.83 north; 594503.78 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:46:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868406.50 2024-11-06T04:46:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:50Z- waiting for 1730868410.50, time is 1730868410.53 2024-11-06T04:46:50Z- LcmTrn::updateTrn() >>>> pose time 1730868410.53, meas time 1730868410.17 <<<< 2024-11-06T04:46:50Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868410.53 2024-11-06T04:46:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868410.17 sec, ping # 0; waiting for INS data more recent than 1730868406.50... 2024-11-06T04:46:50Z- TerrainNav::motionUpdate() - currEstimate = (4061692, 594503.78, 49.49) 2024-11-06T04:46:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:46:50Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:46:50Z- TNavPF::Projecting Measurements 2024-11-06T04:46:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:46:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:46:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:46:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:46:50Z- TNavFilter::4 good beams. 2024-11-06T04:46:50Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:46:50Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:46:50Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:46:50Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:46:50Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:46:50Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:46:50Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:46:50Z- TerrainMapOctree: (vehN, vehE ) = (4061691.80, 594503.57). 2024-11-06T04:46:50Z- TerrainMapOctree: Min Distance = 253.21. 2024-11-06T04:46:50Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:46:50Z- Forcing Subcloud Comparison 2024-11-06T04:46:50Z- Weighting particles with subcloud comparison 2024-11-06T04:46:50Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:50Z- meanExpectedMeasDiff: 0.740450 delta_rms_squared: 0.935659 alpha: 0.979072 2024-11-06T04:46:50Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:50Z- meanExpectedMeasDiff: 0.221931 delta_rms_squared: 0.652589 alpha: 0.970264 2024-11-06T04:46:50Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:50Z- meanExpectedMeasDiff: 1.855693 delta_rms_squared: 0.727113 alpha: 0.973230 2024-11-06T04:46:50Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:50Z- meanExpectedMeasDiff: -3.216596 delta_rms_squared: 0.824881 alpha: 0.976328 2024-11-06T04:46:50Z- Subcloud NIS: 0.520658 2024-11-06T04:46:50Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:46:50Z- TNavPF:: sumWeights = 0.104678 2024-11-06T04:46:50Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:46:50Z- TNavPF::First Term of Map Covariance Matrix is 0.11 2024-11-06T04:46:50Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T04:46:50Z- TerrainNav::Resampling particle filter... 2024-11-06T04:46:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868410.17 sec. 2024-11-06T04:46:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:46:50Z- variances([0], [2], and psi_berg) = 0.298079 0.467086 0.000000 2024-11-06T04:46:50Z- publishEstimatesDecoder() - at 1730868410.53 sec 2024-11-06T04:46:50Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:46:50Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061635.91 594507.35 56.61 2024-11-06T04:46:50Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061635.56 594508.28 56.78 lat/lon: 36.70 -121.94 2024-11-06T04:46:50Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.30 0.47 0.57 0.00 2024-11-06T04:46:50Z- AHRS_M2 msg: 1730868410.91 epoch sec; seqNo:1596 2024-11-06T04:46:50Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.52 psi 2024-11-06T04:46:50Z- handleDepth()- Depth_Keller msg: 1730868410.94 epoch sec; seqNo:1717; depth 49.49 2024-11-06T04:46:50Z- handleDVL() - DVL msg epochMillisec: 1730868410.94 2024-11-06T04:46:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:46:50Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:46:50Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:46:50Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:46:50Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:46:50Z- handleDvl() - RDI_Pathfinder msg: 1730868410.94 sec; seqNo:622; #beams:0 2024-11-06T04:46:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:46:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.08 , -0.02 2024-11-06T04:46:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868410.97 epoch sec; seqNo:1717; 4061691.85 north; 594505.00 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:46:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868410.53 2024-11-06T04:46:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:51Z- waiting for 1730868414.53, time is 1730868410.97 2024-11-06T04:46:51Z- AHRS_M2 msg: 1730868411.32 epoch sec; seqNo:1597 2024-11-06T04:46:51Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.52 psi 2024-11-06T04:46:51Z- handleDepth()- Depth_Keller msg: 1730868411.33 epoch sec; seqNo:1718; depth 49.49 2024-11-06T04:46:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868411.34 epoch sec; seqNo:1718; 4061691.85 north; 594505.00 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:46:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868410.53 2024-11-06T04:46:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:51Z- waiting for 1730868414.53, time is 1730868411.34 2024-11-06T04:46:51Z- AHRS_M2 msg: 1730868411.72 epoch sec; seqNo:1598 2024-11-06T04:46:51Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.52 psi 2024-11-06T04:46:51Z- handleDepth()- Depth_Keller msg: 1730868411.73 epoch sec; seqNo:1719; depth 49.52 2024-11-06T04:46:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868411.75 epoch sec; seqNo:1719; 4061691.85 north; 594505.00 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:46:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868410.53 2024-11-06T04:46:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:51Z- waiting for 1730868414.53, time is 1730868411.75 2024-11-06T04:46:52Z- AHRS_M2 msg: 1730868412.15 epoch sec; seqNo:1599 2024-11-06T04:46:52Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.52 psi 2024-11-06T04:46:52Z- handleDepth()- Depth_Keller msg: 1730868412.16 epoch sec; seqNo:1720; depth 49.53 2024-11-06T04:46:52Z- handleDVL() - DVL msg epochMillisec: 1730868412.18 2024-11-06T04:46:52Z- handleDvl() - RDI_Pathfinder msg: 1730868412.18 sec; seqNo:623; #beams:4 2024-11-06T04:46:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.10 , 32.45 , 35.85 , 33.64 2024-11-06T04:46:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.07 , 0.02 2024-11-06T04:46:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868412.19 epoch sec; seqNo:1720; 4061691.85 north; 594505.00 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T04:46:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868410.53 2024-11-06T04:46:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:52Z- waiting for 1730868414.53, time is 1730868412.19 2024-11-06T04:46:52Z- AHRS_M2 msg: 1730868412.53 epoch sec; seqNo:1600 2024-11-06T04:46:52Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.52 psi 2024-11-06T04:46:52Z- handleDepth()- Depth_Keller msg: 1730868412.54 epoch sec; seqNo:1721; depth 49.55 2024-11-06T04:46:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868412.55 epoch sec; seqNo:1721; 4061692.24 north; 594506.22 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T04:46:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868410.53 2024-11-06T04:46:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:52Z- waiting for 1730868414.53, time is 1730868412.55 2024-11-06T04:46:52Z- AHRS_M2 msg: 1730868412.94 epoch sec; seqNo:1601 2024-11-06T04:46:52Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.53 psi 2024-11-06T04:46:52Z- handleDepth()- Depth_Keller msg: 1730868412.95 epoch sec; seqNo:1722; depth 49.57 2024-11-06T04:46:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868412.95 epoch sec; seqNo:1722; 4061692.24 north; 594506.22 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T04:46:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868410.53 2024-11-06T04:46:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:53Z- waiting for 1730868414.53, time is 1730868412.95 2024-11-06T04:46:53Z- AHRS_M2 msg: 1730868413.34 epoch sec; seqNo:1602 2024-11-06T04:46:53Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.53 psi 2024-11-06T04:46:53Z- handleDepth()- Depth_Keller msg: 1730868413.36 epoch sec; seqNo:1723; depth 49.59 2024-11-06T04:46:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868413.38 epoch sec; seqNo:1723; 4061692.24 north; 594506.22 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:46:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868410.53 2024-11-06T04:46:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:53Z- waiting for 1730868414.53, time is 1730868413.38 2024-11-06T04:46:53Z- AHRS_M2 msg: 1730868413.74 epoch sec; seqNo:1603 2024-11-06T04:46:53Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.53 psi 2024-11-06T04:46:53Z- handleDepth()- Depth_Keller msg: 1730868413.75 epoch sec; seqNo:1724; depth 49.62 2024-11-06T04:46:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868413.76 epoch sec; seqNo:1724; 4061692.24 north; 594506.22 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:46:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868410.53 2024-11-06T04:46:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:53Z- waiting for 1730868414.53, time is 1730868413.76 2024-11-06T04:46:54Z- AHRS_M2 msg: 1730868414.17 epoch sec; seqNo:1604 2024-11-06T04:46:54Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.53 psi 2024-11-06T04:46:54Z- handleDepth()- Depth_Keller msg: 1730868414.17 epoch sec; seqNo:1725; depth 49.65 2024-11-06T04:46:54Z- handleDVL() - DVL msg epochMillisec: 1730868414.21 2024-11-06T04:46:54Z- handleDvl() - RDI_Pathfinder msg: 1730868414.21 sec; seqNo:624; #beams:4 2024-11-06T04:46:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.10 , 32.62 , 35.68 , 34.15 2024-11-06T04:46:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , 0.03 2024-11-06T04:46:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868414.22 epoch sec; seqNo:1725; 4061692.25 north; 594507.44 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:46:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868410.53 2024-11-06T04:46:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:54Z- waiting for 1730868414.53, time is 1730868414.22 2024-11-06T04:46:54Z- AHRS_M2 msg: 1730868414.56 epoch sec; seqNo:1605 2024-11-06T04:46:54Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.53 psi 2024-11-06T04:46:54Z- handleDepth()- Depth_Keller msg: 1730868414.57 epoch sec; seqNo:1726; depth 49.67 2024-11-06T04:46:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868414.59 epoch sec; seqNo:1726; 4061692.25 north; 594507.44 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:46:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868410.53 2024-11-06T04:46:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:54Z- waiting for 1730868414.53, time is 1730868414.59 2024-11-06T04:46:54Z- LcmTrn::updateTrn() >>>> pose time 1730868414.59, meas time 1730868414.21 <<<< 2024-11-06T04:46:54Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868414.59 2024-11-06T04:46:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868414.21 sec, ping # 0; waiting for INS data more recent than 1730868410.53... 2024-11-06T04:46:54Z- TerrainNav::motionUpdate() - currEstimate = (4061692, 594507.44, 49.67) 2024-11-06T04:46:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:46:54Z- TerrainNav:: SubcloudNIS is 0.52, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:46:54Z- TNavPF::Projecting Measurements 2024-11-06T04:46:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:46:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:46:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:46:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:46:54Z- TNavFilter::4 good beams. 2024-11-06T04:46:54Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:46:54Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:46:54Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:46:54Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:46:54Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:46:54Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:46:54Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:46:54Z- TerrainMapOctree: (vehN, vehE ) = (4061692.21, 594507.10). 2024-11-06T04:46:54Z- TerrainMapOctree: Min Distance = 249.68. 2024-11-06T04:46:54Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:46:54Z- Forcing Subcloud Comparison 2024-11-06T04:46:54Z- Weighting particles with subcloud comparison 2024-11-06T04:46:54Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:54Z- meanExpectedMeasDiff: 1.672742 delta_rms_squared: 2.357865 alpha: 0.991589 2024-11-06T04:46:54Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:54Z- meanExpectedMeasDiff: 0.581411 delta_rms_squared: 0.954497 alpha: 0.979477 2024-11-06T04:46:54Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:54Z- meanExpectedMeasDiff: 2.014517 delta_rms_squared: 1.054803 alpha: 0.981392 2024-11-06T04:46:54Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:54Z- meanExpectedMeasDiff: -1.781135 delta_rms_squared: 1.203013 alpha: 0.983647 2024-11-06T04:46:54Z- Subcloud NIS: 0.535709 2024-11-06T04:46:54Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:46:54Z- TNavPF:: sumWeights = 0.184918 2024-11-06T04:46:54Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:46:54Z- TNavPF::First Term of Map Covariance Matrix is 0.40 2024-11-06T04:46:54Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.02 2024-11-06T04:46:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868414.21 sec. 2024-11-06T04:46:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:46:54Z- variances([0], [2], and psi_berg) = 0.315265 0.492372 0.000000 2024-11-06T04:46:54Z- publishEstimatesDecoder() - at 1730868414.59 sec 2024-11-06T04:46:54Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:46:54Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061636.37 594511.17 56.26 2024-11-06T04:46:54Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061636.03 594511.86 56.68 lat/lon: 36.70 -121.94 2024-11-06T04:46:54Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.32 0.49 0.61 0.00 2024-11-06T04:46:54Z- AHRS_M2 msg: 1730868414.96 epoch sec; seqNo:1606 2024-11-06T04:46:54Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.53 psi 2024-11-06T04:46:54Z- handleDepth()- Depth_Keller msg: 1730868414.96 epoch sec; seqNo:1727; depth 49.71 2024-11-06T04:46:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868414.99 epoch sec; seqNo:1727; 4061692.25 north; 594507.44 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T04:46:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868414.59 2024-11-06T04:46:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:55Z- waiting for 1730868418.59, time is 1730868414.99 2024-11-06T04:46:55Z- AHRS_M2 msg: 1730868415.38 epoch sec; seqNo:1607 2024-11-06T04:46:55Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.54 psi 2024-11-06T04:46:55Z- handleDepth()- Depth_Keller msg: 1730868415.38 epoch sec; seqNo:1728; depth 49.72 2024-11-06T04:46:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868415.39 epoch sec; seqNo:1728; 4061692.25 north; 594507.44 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:46:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868414.59 2024-11-06T04:46:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:55Z- waiting for 1730868418.59, time is 1730868415.39 2024-11-06T04:46:55Z- AHRS_M2 msg: 1730868415.78 epoch sec; seqNo:1608 2024-11-06T04:46:55Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.54 psi 2024-11-06T04:46:55Z- handleDepth()- Depth_Keller msg: 1730868415.78 epoch sec; seqNo:1729; depth 49.75 2024-11-06T04:46:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868415.79 epoch sec; seqNo:1729; 4061692.27 north; 594508.66 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:46:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868414.59 2024-11-06T04:46:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:55Z- waiting for 1730868418.59, time is 1730868415.79 2024-11-06T04:46:56Z- AHRS_M2 msg: 1730868416.17 epoch sec; seqNo:1609 2024-11-06T04:46:56Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.54 psi 2024-11-06T04:46:56Z- handleDepth()- Depth_Keller msg: 1730868416.19 epoch sec; seqNo:1730; depth 49.77 2024-11-06T04:46:56Z- handleDVL() - DVL msg epochMillisec: 1730868416.21 2024-11-06T04:46:56Z- handleDvl() - RDI_Pathfinder msg: 1730868416.21 sec; seqNo:625; #beams:4 2024-11-06T04:46:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.27 , 32.45 , 35.85 , 34.66 2024-11-06T04:46:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.06 2024-11-06T04:46:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868416.22 epoch sec; seqNo:1730; 4061692.27 north; 594508.66 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T04:46:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868414.59 2024-11-06T04:46:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:56Z- waiting for 1730868418.59, time is 1730868416.22 2024-11-06T04:46:56Z- AHRS_M2 msg: 1730868416.57 epoch sec; seqNo:1610 2024-11-06T04:46:56Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.54 psi 2024-11-06T04:46:56Z- handleDepth()- Depth_Keller msg: 1730868416.58 epoch sec; seqNo:1731; depth 49.81 2024-11-06T04:46:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868416.59 epoch sec; seqNo:1731; 4061692.64 north; 594508.65 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:46:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868414.59 2024-11-06T04:46:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:56Z- waiting for 1730868418.59, time is 1730868416.59 2024-11-06T04:46:56Z- AHRS_M2 msg: 1730868416.98 epoch sec; seqNo:1611 2024-11-06T04:46:56Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.54 psi 2024-11-06T04:46:56Z- handleDepth()- Depth_Keller msg: 1730868416.99 epoch sec; seqNo:1732; depth 49.82 2024-11-06T04:46:56Z- handleDVL() - DVL msg epochMillisec: 1730868416.99 2024-11-06T04:46:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:46:56Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:46:56Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:46:56Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:46:56Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:46:56Z- handleDvl() - RDI_Pathfinder msg: 1730868416.99 sec; seqNo:626; #beams:0 2024-11-06T04:46:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:46:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.06 2024-11-06T04:46:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868417.00 epoch sec; seqNo:1732; 4061692.66 north; 594509.87 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:46:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868414.59 2024-11-06T04:46:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:57Z- waiting for 1730868418.59, time is 1730868417.00 2024-11-06T04:46:57Z- AHRS_M2 msg: 1730868417.38 epoch sec; seqNo:1612 2024-11-06T04:46:57Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.54 psi 2024-11-06T04:46:57Z- handleDepth()- Depth_Keller msg: 1730868417.40 epoch sec; seqNo:1733; depth 49.84 2024-11-06T04:46:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868417.41 epoch sec; seqNo:1733; 4061692.66 north; 594509.87 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T04:46:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868414.59 2024-11-06T04:46:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:57Z- waiting for 1730868418.59, time is 1730868417.41 2024-11-06T04:46:57Z- AHRS_M2 msg: 1730868417.78 epoch sec; seqNo:1613 2024-11-06T04:46:57Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.54 psi 2024-11-06T04:46:57Z- handleDepth()- Depth_Keller msg: 1730868417.79 epoch sec; seqNo:1734; depth 49.86 2024-11-06T04:46:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868417.80 epoch sec; seqNo:1734; 4061692.66 north; 594509.87 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:46:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868414.59 2024-11-06T04:46:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:57Z- waiting for 1730868418.59, time is 1730868417.80 2024-11-06T04:46:58Z- AHRS_M2 msg: 1730868418.19 epoch sec; seqNo:1614 2024-11-06T04:46:58Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.54 psi 2024-11-06T04:46:58Z- handleDepth()- Depth_Keller msg: 1730868418.20 epoch sec; seqNo:1735; depth 49.86 2024-11-06T04:46:58Z- handleDVL() - DVL msg epochMillisec: 1730868418.23 2024-11-06T04:46:58Z- handleDvl() - RDI_Pathfinder msg: 1730868418.23 sec; seqNo:627; #beams:4 2024-11-06T04:46:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.78 , 32.45 , 35.85 , 35.34 2024-11-06T04:46:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , 0.08 2024-11-06T04:46:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868418.24 epoch sec; seqNo:1735; 4061692.66 north; 594509.87 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:46:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868414.59 2024-11-06T04:46:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:58Z- waiting for 1730868418.59, time is 1730868418.24 2024-11-06T04:46:58Z- AHRS_M2 msg: 1730868418.59 epoch sec; seqNo:1615 2024-11-06T04:46:58Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.54 psi 2024-11-06T04:46:58Z- handleDepth()- Depth_Keller msg: 1730868418.60 epoch sec; seqNo:1736; depth 49.88 2024-11-06T04:46:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868418.61 epoch sec; seqNo:1736; 4061692.67 north; 594511.10 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T04:46:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868414.59 2024-11-06T04:46:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:58Z- waiting for 1730868418.59, time is 1730868418.61 2024-11-06T04:46:58Z- LcmTrn::updateTrn() >>>> pose time 1730868418.61, meas time 1730868418.23 <<<< 2024-11-06T04:46:58Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868418.61 2024-11-06T04:46:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868418.23 sec, ping # 0; waiting for INS data more recent than 1730868414.59... 2024-11-06T04:46:58Z- TerrainNav::motionUpdate() - currEstimate = (4061693, 594511.10, 49.88) 2024-11-06T04:46:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:46:58Z- TerrainNav:: SubcloudNIS is 0.54, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:46:58Z- TNavPF::Projecting Measurements 2024-11-06T04:46:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:46:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:46:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:46:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:46:58Z- TNavFilter::4 good beams. 2024-11-06T04:46:58Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:46:58Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:46:58Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:46:58Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:46:58Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:46:58Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:46:58Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:46:58Z- TerrainMapOctree: (vehN, vehE ) = (4061692.63, 594510.75). 2024-11-06T04:46:58Z- TerrainMapOctree: Min Distance = 246.03. 2024-11-06T04:46:58Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:46:58Z- Forcing Subcloud Comparison 2024-11-06T04:46:58Z- Weighting particles with subcloud comparison 2024-11-06T04:46:58Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:58Z- meanExpectedMeasDiff: 2.858068 delta_rms_squared: 1.677606 alpha: 0.988219 2024-11-06T04:46:58Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:58Z- meanExpectedMeasDiff: 0.229951 delta_rms_squared: 0.988898 alpha: 0.980176 2024-11-06T04:46:58Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:58Z- meanExpectedMeasDiff: 2.233745 delta_rms_squared: 1.077210 alpha: 0.981772 2024-11-06T04:46:58Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:46:58Z- meanExpectedMeasDiff: -0.740643 delta_rms_squared: 1.242458 alpha: 0.984158 2024-11-06T04:46:58Z- Subcloud NIS: 0.534765 2024-11-06T04:46:58Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:46:58Z- TNavPF:: sumWeights = 0.179998 2024-11-06T04:46:58Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:46:58Z- TNavPF::First Term of Map Covariance Matrix is 0.71 2024-11-06T04:46:58Z- TNavPF::Calculated NIS Value : 0.09 numBeams normalized NIS: 0.02 2024-11-06T04:46:58Z- TerrainNav::Resampling particle filter... 2024-11-06T04:46:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868418.23 sec. 2024-11-06T04:46:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:46:58Z- variances([0], [2], and psi_berg) = 0.335658 0.502485 0.000000 2024-11-06T04:46:58Z- publishEstimatesDecoder() - at 1730868418.61 sec 2024-11-06T04:46:58Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:46:58Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061636.76 594514.73 57.48 2024-11-06T04:46:58Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061636.46 594515.50 56.38 lat/lon: 36.70 -121.94 2024-11-06T04:46:58Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.34 0.50 0.63 0.00 2024-11-06T04:46:59Z- AHRS_M2 msg: 1730868418.99 epoch sec; seqNo:1616 2024-11-06T04:46:59Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.54 psi 2024-11-06T04:46:59Z- handleDepth()- Depth_Keller msg: 1730868419.03 epoch sec; seqNo:1737; depth 49.87 2024-11-06T04:46:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868419.04 epoch sec; seqNo:1737; 4061692.67 north; 594511.10 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:46:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868418.61 2024-11-06T04:46:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:59Z- waiting for 1730868422.61, time is 1730868419.04 2024-11-06T04:46:59Z- AHRS_M2 msg: 1730868419.40 epoch sec; seqNo:1617 2024-11-06T04:46:59Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 1.54 psi 2024-11-06T04:46:59Z- handleDepth()- Depth_Keller msg: 1730868419.41 epoch sec; seqNo:1738; depth 49.89 2024-11-06T04:46:59Z- handleDVL() - DVL msg epochMillisec: 1730868419.41 2024-11-06T04:46:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:46:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:46:59Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:46:59Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:46:59Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:46:59Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:46:59Z- handleDvl() - RDI_Pathfinder msg: 1730868419.41 sec; seqNo:628; #beams:0 2024-11-06T04:46:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:46:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , 0.08 2024-11-06T04:46:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868419.43 epoch sec; seqNo:1738; 4061692.67 north; 594511.10 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:46:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:46:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868418.61 2024-11-06T04:46:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:59Z- waiting for 1730868422.61, time is 1730868419.43 2024-11-06T04:46:59Z- AHRS_M2 msg: 1730868419.82 epoch sec; seqNo:1618 2024-11-06T04:46:59Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 1.55 psi 2024-11-06T04:46:59Z- handleDepth()- Depth_Keller msg: 1730868419.83 epoch sec; seqNo:1739; depth 49.91 2024-11-06T04:46:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868419.84 epoch sec; seqNo:1739; 4061692.68 north; 594512.32 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:46:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:46:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868418.61 2024-11-06T04:46:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:46:59Z- waiting for 1730868422.61, time is 1730868419.84 2024-11-06T04:47:00Z- AHRS_M2 msg: 1730868420.21 epoch sec; seqNo:1619 2024-11-06T04:47:00Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 1.55 psi 2024-11-06T04:47:00Z- handleDepth()- Depth_Keller msg: 1730868420.22 epoch sec; seqNo:1740; depth 49.91 2024-11-06T04:47:00Z- handleDVL() - DVL msg epochMillisec: 1730868420.25 2024-11-06T04:47:00Z- handleDvl() - RDI_Pathfinder msg: 1730868420.25 sec; seqNo:629; #beams:4 2024-11-06T04:47:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.10 , 32.96 , 35.68 , 35.51 2024-11-06T04:47:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.02 , 0.02 2024-11-06T04:47:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868420.26 epoch sec; seqNo:1740; 4061692.68 north; 594512.32 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:47:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868418.61 2024-11-06T04:47:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:00Z- waiting for 1730868422.61, time is 1730868420.26 2024-11-06T04:47:00Z- AHRS_M2 msg: 1730868420.63 epoch sec; seqNo:1620 2024-11-06T04:47:00Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 1.55 psi 2024-11-06T04:47:00Z- handleDepth()- Depth_Keller msg: 1730868420.65 epoch sec; seqNo:1741; depth 49.91 2024-11-06T04:47:00Z- handleDVL() - DVL msg epochMillisec: 1730868420.65 2024-11-06T04:47:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:00Z- handleDvl() - RDI_Pathfinder msg: 1730868420.65 sec; seqNo:630; #beams:0 2024-11-06T04:47:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.02 , 0.02 2024-11-06T04:47:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868420.66 epoch sec; seqNo:1741; 4061692.68 north; 594512.32 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:47:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868418.61 2024-11-06T04:47:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:00Z- waiting for 1730868422.61, time is 1730868420.66 2024-11-06T04:47:01Z- AHRS_M2 msg: 1730868421.02 epoch sec; seqNo:1621 2024-11-06T04:47:01Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.55 psi 2024-11-06T04:47:01Z- handleDepth()- Depth_Keller msg: 1730868421.03 epoch sec; seqNo:1742; depth 49.92 2024-11-06T04:47:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868421.04 epoch sec; seqNo:1742; 4061692.70 north; 594513.54 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:47:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868418.61 2024-11-06T04:47:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:01Z- waiting for 1730868422.61, time is 1730868421.04 2024-11-06T04:47:01Z- AHRS_M2 msg: 1730868421.43 epoch sec; seqNo:1622 2024-11-06T04:47:01Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.56 psi 2024-11-06T04:47:01Z- handleDepth()- Depth_Keller msg: 1730868421.43 epoch sec; seqNo:1743; depth 49.91 2024-11-06T04:47:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868421.44 epoch sec; seqNo:1743; 4061692.70 north; 594513.54 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:47:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868418.61 2024-11-06T04:47:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:01Z- waiting for 1730868422.61, time is 1730868421.44 2024-11-06T04:47:01Z- AHRS_M2 msg: 1730868421.83 epoch sec; seqNo:1623 2024-11-06T04:47:01Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.56 psi 2024-11-06T04:47:01Z- handleDepth()- Depth_Keller msg: 1730868421.85 epoch sec; seqNo:1744; depth 49.91 2024-11-06T04:47:01Z- handleDVL() - DVL msg epochMillisec: 1730868421.85 2024-11-06T04:47:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:01Z- handleDvl() - RDI_Pathfinder msg: 1730868421.85 sec; seqNo:631; #beams:0 2024-11-06T04:47:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.02 , 0.02 2024-11-06T04:47:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868421.86 epoch sec; seqNo:1744; 4061692.70 north; 594513.54 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:47:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868418.61 2024-11-06T04:47:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:01Z- waiting for 1730868422.61, time is 1730868421.86 2024-11-06T04:47:02Z- AHRS_M2 msg: 1730868422.23 epoch sec; seqNo:1624 2024-11-06T04:47:02Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.56 psi 2024-11-06T04:47:02Z- handleDepth()- Depth_Keller msg: 1730868422.24 epoch sec; seqNo:1745; depth 49.90 2024-11-06T04:47:02Z- handleDVL() - DVL msg epochMillisec: 1730868422.28 2024-11-06T04:47:02Z- handleDvl() - RDI_Pathfinder msg: 1730868422.28 sec; seqNo:632; #beams:4 2024-11-06T04:47:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.93 , 32.96 , 35.85 , 35.85 2024-11-06T04:47:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.10 2024-11-06T04:47:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868422.30 epoch sec; seqNo:1745; 4061692.71 north; 594514.76 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:47:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868418.61 2024-11-06T04:47:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:02Z- waiting for 1730868422.61, time is 1730868422.30 2024-11-06T04:47:02Z- AHRS_M2 msg: 1730868422.63 epoch sec; seqNo:1625 2024-11-06T04:47:02Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.56 psi 2024-11-06T04:47:02Z- handleDepth()- Depth_Keller msg: 1730868422.64 epoch sec; seqNo:1746; depth 49.91 2024-11-06T04:47:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868422.65 epoch sec; seqNo:1746; 4061692.71 north; 594514.76 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:47:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868418.61 2024-11-06T04:47:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:02Z- waiting for 1730868422.61, time is 1730868422.65 2024-11-06T04:47:02Z- LcmTrn::updateTrn() >>>> pose time 1730868422.65, meas time 1730868422.28 <<<< 2024-11-06T04:47:02Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868422.65 2024-11-06T04:47:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868422.28 sec, ping # 0; waiting for INS data more recent than 1730868418.61... 2024-11-06T04:47:02Z- TerrainNav::motionUpdate() - currEstimate = (4061693, 594514.76, 49.91) 2024-11-06T04:47:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:47:02Z- TerrainNav:: SubcloudNIS is 0.53, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:47:02Z- TNavPF::Projecting Measurements 2024-11-06T04:47:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:47:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:47:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:47:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:47:02Z- TNavFilter::4 good beams. 2024-11-06T04:47:02Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:47:02Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:47:02Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:47:02Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:47:02Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:47:02Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:47:02Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:47:02Z- TerrainMapOctree: (vehN, vehE ) = (4061692.71, 594514.42). 2024-11-06T04:47:02Z- TerrainMapOctree: Min Distance = 242.45. 2024-11-06T04:47:02Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:47:02Z- Forcing Subcloud Comparison 2024-11-06T04:47:02Z- Weighting particles with subcloud comparison 2024-11-06T04:47:02Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:02Z- meanExpectedMeasDiff: 1.457035 delta_rms_squared: 1.415298 alpha: 0.986066 2024-11-06T04:47:02Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:02Z- meanExpectedMeasDiff: 0.248344 delta_rms_squared: 1.020618 alpha: 0.980781 2024-11-06T04:47:02Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:02Z- meanExpectedMeasDiff: 1.682777 delta_rms_squared: 1.114240 alpha: 0.982367 2024-11-06T04:47:02Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:02Z- meanExpectedMeasDiff: -0.787979 delta_rms_squared: 1.282714 alpha: 0.984647 2024-11-06T04:47:02Z- Subcloud NIS: 0.520955 2024-11-06T04:47:02Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:47:02Z- TNavPF:: sumWeights = 0.369829 2024-11-06T04:47:02Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:47:02Z- TNavPF::First Term of Map Covariance Matrix is 0.41 2024-11-06T04:47:02Z- TNavPF::Calculated NIS Value : 0.21 numBeams normalized NIS: 0.05 2024-11-06T04:47:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868422.28 sec. 2024-11-06T04:47:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:47:02Z- variances([0], [2], and psi_berg) = 0.334563 0.506970 0.000000 2024-11-06T04:47:02Z- publishEstimatesDecoder() - at 1730868422.65 sec 2024-11-06T04:47:02Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:47:02Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061636.35 594519.12 54.86 2024-11-06T04:47:02Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061636.50 594519.16 56.12 lat/lon: 36.70 -121.94 2024-11-06T04:47:02Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.51 0.65 0.00 2024-11-06T04:47:03Z- AHRS_M2 msg: 1730868423.04 epoch sec; seqNo:1626 2024-11-06T04:47:03Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.57 psi 2024-11-06T04:47:03Z- handleDepth()- Depth_Keller msg: 1730868423.05 epoch sec; seqNo:1747; depth 49.90 2024-11-06T04:47:03Z- handleDVL() - DVL msg epochMillisec: 1730868423.05 2024-11-06T04:47:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:03Z- handleDvl() - RDI_Pathfinder msg: 1730868423.05 sec; seqNo:633; #beams:0 2024-11-06T04:47:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.10 2024-11-06T04:47:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868423.06 epoch sec; seqNo:1747; 4061692.71 north; 594514.76 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:47:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868422.65 2024-11-06T04:47:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:03Z- waiting for 1730868426.65, time is 1730868423.06 2024-11-06T04:47:03Z- AHRS_M2 msg: 1730868423.44 epoch sec; seqNo:1627 2024-11-06T04:47:03Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.57 psi 2024-11-06T04:47:03Z- handleDepth()- Depth_Keller msg: 1730868423.45 epoch sec; seqNo:1748; depth 49.90 2024-11-06T04:47:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868423.48 epoch sec; seqNo:1748; 4061692.71 north; 594514.76 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:47:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868422.65 2024-11-06T04:47:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:03Z- waiting for 1730868426.65, time is 1730868423.48 2024-11-06T04:47:03Z- AHRS_M2 msg: 1730868423.85 epoch sec; seqNo:1628 2024-11-06T04:47:03Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.57 psi 2024-11-06T04:47:03Z- handleDepth()- Depth_Keller msg: 1730868423.85 epoch sec; seqNo:1749; depth 49.90 2024-11-06T04:47:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868423.86 epoch sec; seqNo:1749; 4061692.73 north; 594515.98 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:47:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868422.65 2024-11-06T04:47:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:04Z- waiting for 1730868426.65, time is 1730868423.86 2024-11-06T04:47:04Z- AHRS_M2 msg: 1730868424.26 epoch sec; seqNo:1629 2024-11-06T04:47:04Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.57 psi 2024-11-06T04:47:04Z- handleDepth()- Depth_Keller msg: 1730868424.27 epoch sec; seqNo:1750; depth 49.90 2024-11-06T04:47:04Z- handleDVL() - DVL msg epochMillisec: 1730868424.31 2024-11-06T04:47:04Z- handleDvl() - RDI_Pathfinder msg: 1730868424.31 sec; seqNo:634; #beams:4 2024-11-06T04:47:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.76 , 32.79 , 35.51 , 35.51 2024-11-06T04:47:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.07 , -0.15 2024-11-06T04:47:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868424.33 epoch sec; seqNo:1750; 4061692.73 north; 594515.98 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:47:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868422.65 2024-11-06T04:47:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:04Z- waiting for 1730868426.65, time is 1730868424.33 2024-11-06T04:47:04Z- AHRS_M2 msg: 1730868424.65 epoch sec; seqNo:1630 2024-11-06T04:47:04Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.57 psi 2024-11-06T04:47:04Z- handleDepth()- Depth_Keller msg: 1730868424.66 epoch sec; seqNo:1751; depth 49.91 2024-11-06T04:47:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868424.69 epoch sec; seqNo:1751; 4061692.73 north; 594515.98 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:47:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868422.65 2024-11-06T04:47:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:04Z- waiting for 1730868426.65, time is 1730868424.69 2024-11-06T04:47:05Z- AHRS_M2 msg: 1730868425.06 epoch sec; seqNo:1631 2024-11-06T04:47:05Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 1.56 psi 2024-11-06T04:47:05Z- handleDepth()- Depth_Keller msg: 1730868425.06 epoch sec; seqNo:1752; depth 49.92 2024-11-06T04:47:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868425.07 epoch sec; seqNo:1752; 4061692.73 north; 594515.98 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:47:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868422.65 2024-11-06T04:47:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:05Z- waiting for 1730868426.65, time is 1730868425.07 2024-11-06T04:47:05Z- AHRS_M2 msg: 1730868425.47 epoch sec; seqNo:1632 2024-11-06T04:47:05Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 1.56 psi 2024-11-06T04:47:05Z- handleDepth()- Depth_Keller msg: 1730868425.47 epoch sec; seqNo:1753; depth 49.93 2024-11-06T04:47:05Z- handleDVL() - DVL msg epochMillisec: 1730868425.48 2024-11-06T04:47:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:05Z- handleDvl() - RDI_Pathfinder msg: 1730868425.48 sec; seqNo:635; #beams:0 2024-11-06T04:47:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.73 , -0.07 , -0.15 2024-11-06T04:47:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868425.48 epoch sec; seqNo:1753; 4061692.74 north; 594517.20 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T04:47:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868422.65 2024-11-06T04:47:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:05Z- waiting for 1730868426.65, time is 1730868425.48 2024-11-06T04:47:05Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:47:05Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:47:05Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:47:05Z- AHRS_M2 msg: 1730868425.86 epoch sec; seqNo:1633 2024-11-06T04:47:05Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 1.56 psi 2024-11-06T04:47:05Z- handleDepth()- Depth_Keller msg: 1730868425.89 epoch sec; seqNo:1754; depth 49.95 2024-11-06T04:47:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868425.90 epoch sec; seqNo:1754; 4061692.74 north; 594517.20 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T04:47:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868422.65 2024-11-06T04:47:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:06Z- waiting for 1730868426.65, time is 1730868425.90 2024-11-06T04:47:06Z- AHRS_M2 msg: 1730868426.27 epoch sec; seqNo:1634 2024-11-06T04:47:06Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 1.55 psi 2024-11-06T04:47:06Z- handleDepth()- Depth_Keller msg: 1730868426.28 epoch sec; seqNo:1755; depth 49.96 2024-11-06T04:47:06Z- handleDVL() - DVL msg epochMillisec: 1730868426.31 2024-11-06T04:47:06Z- handleDvl() - RDI_Pathfinder msg: 1730868426.31 sec; seqNo:636; #beams:4 2024-11-06T04:47:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.25 , 32.28 , 35.17 , 35.68 2024-11-06T04:47:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.12 , -0.04 2024-11-06T04:47:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868426.34 epoch sec; seqNo:1755; 4061692.74 north; 594517.20 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:47:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868422.65 2024-11-06T04:47:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:06Z- waiting for 1730868426.65, time is 1730868426.34 2024-11-06T04:47:06Z- AHRS_M2 msg: 1730868426.71 epoch sec; seqNo:1635 2024-11-06T04:47:06Z- AHRS_M2 msg: 0.13 phi; -0.06 theta; 1.55 psi 2024-11-06T04:47:06Z- handleDepth()- Depth_Keller msg: 1730868426.71 epoch sec; seqNo:1756; depth 49.98 2024-11-06T04:47:06Z- handleDVL() - DVL msg epochMillisec: 1730868426.71 2024-11-06T04:47:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:06Z- handleDvl() - RDI_Pathfinder msg: 1730868426.71 sec; seqNo:637; #beams:0 2024-11-06T04:47:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.12 , -0.04 2024-11-06T04:47:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868426.73 epoch sec; seqNo:1756; 4061693.12 north; 594517.19 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T04:47:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868422.65 2024-11-06T04:47:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:06Z- waiting for 1730868426.65, time is 1730868426.73 2024-11-06T04:47:06Z- LcmTrn::updateTrn() >>>> pose time 1730868426.73, meas time 1730868426.71 <<<< 2024-11-06T04:47:06Z- LcmTrn::updateTrn() equating pose 1730868426.71 and meas times 1730868426.71 2024-11-06T04:47:06Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868426.71 2024-11-06T04:47:06Z- TerrainNav::motionUpdate() - currEstimate = (4061693, 594517.19, 49.98) 2024-11-06T04:47:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:47:06Z- TerrainNav:: SubcloudNIS is 0.52, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:47:06Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:47:06Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:47:06Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:47:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:47:06Z- variances([0], [2], and psi_berg) = 0.335166 0.508308 0.000000 2024-11-06T04:47:06Z- publishEstimatesDecoder() - at 1730868426.71 sec 2024-11-06T04:47:06Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:47:06Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061636.77 594521.54 54.29 2024-11-06T04:47:06Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061636.90 594521.60 56.19 lat/lon: 36.70 -121.94 2024-11-06T04:47:06Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.34 0.51 0.91 0.00 2024-11-06T04:47:07Z- AHRS_M2 msg: 1730868427.08 epoch sec; seqNo:1636 2024-11-06T04:47:07Z- AHRS_M2 msg: 0.12 phi; -0.06 theta; 1.54 psi 2024-11-06T04:47:07Z- handleDepth()- Depth_Keller msg: 1730868427.09 epoch sec; seqNo:1757; depth 50.01 2024-11-06T04:47:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868427.11 epoch sec; seqNo:1757; 4061693.13 north; 594518.41 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:47:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868426.71 2024-11-06T04:47:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:07Z- waiting for 1730868430.71, time is 1730868427.11 2024-11-06T04:47:07Z- handleDepth()- Depth_Keller msg: 1730868427.50 epoch sec; seqNo:1758; depth 50.03 2024-11-06T04:47:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868427.52 epoch sec; seqNo:1758; 4061693.13 north; 594518.41 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T04:47:07Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:47:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868426.71 2024-11-06T04:47:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:07Z- waiting for 1730868430.71, time is 1730868427.52 2024-11-06T04:47:07Z- AHRS_M2 msg: 1730868427.89 epoch sec; seqNo:1637 2024-11-06T04:47:07Z- AHRS_M2 msg: 0.11 phi; -0.06 theta; 1.53 psi 2024-11-06T04:47:07Z- handleDepth()- Depth_Keller msg: 1730868427.89 epoch sec; seqNo:1759; depth 50.08 2024-11-06T04:47:07Z- handleDVL() - DVL msg epochMillisec: 1730868427.89 2024-11-06T04:47:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:07Z- handleDvl() - RDI_Pathfinder msg: 1730868427.89 sec; seqNo:638; #beams:0 2024-11-06T04:47:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.64 , -0.12 , -0.04 2024-11-06T04:47:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868427.90 epoch sec; seqNo:1759; 4061693.13 north; 594518.41 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T04:47:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868426.71 2024-11-06T04:47:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:08Z- waiting for 1730868430.71, time is 1730868427.90 2024-11-06T04:47:08Z- AHRS_M2 msg: 1730868428.30 epoch sec; seqNo:1638 2024-11-06T04:47:08Z- AHRS_M2 msg: 0.11 phi; -0.06 theta; 1.52 psi 2024-11-06T04:47:08Z- handleDepth()- Depth_Keller msg: 1730868428.31 epoch sec; seqNo:1760; depth 50.09 2024-11-06T04:47:08Z- handleDVL() - DVL msg epochMillisec: 1730868428.33 2024-11-06T04:47:08Z- handleDvl() - RDI_Pathfinder msg: 1730868428.33 sec; seqNo:639; #beams:4 2024-11-06T04:47:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.08 , 32.28 , 34.83 , 35.68 2024-11-06T04:47:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.10 , 0.13 2024-11-06T04:47:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868428.37 epoch sec; seqNo:1760; 4061693.13 north; 594518.41 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T04:47:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868426.71 2024-11-06T04:47:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:08Z- waiting for 1730868430.71, time is 1730868428.37 2024-11-06T04:47:08Z- AHRS_M2 msg: 1730868428.69 epoch sec; seqNo:1639 2024-11-06T04:47:08Z- AHRS_M2 msg: 0.11 phi; -0.06 theta; 1.52 psi 2024-11-06T04:47:08Z- handleDepth()- Depth_Keller msg: 1730868428.70 epoch sec; seqNo:1761; depth 50.14 2024-11-06T04:47:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868428.72 epoch sec; seqNo:1761; 4061693.13 north; 594518.41 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:47:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868426.71 2024-11-06T04:47:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:08Z- waiting for 1730868430.71, time is 1730868428.72 2024-11-06T04:47:09Z- AHRS_M2 msg: 1730868429.10 epoch sec; seqNo:1640 2024-11-06T04:47:09Z- AHRS_M2 msg: 0.11 phi; -0.06 theta; 1.51 psi 2024-11-06T04:47:09Z- handleDepth()- Depth_Keller msg: 1730868429.11 epoch sec; seqNo:1762; depth 50.16 2024-11-06T04:47:09Z- handleDVL() - DVL msg epochMillisec: 1730868429.11 2024-11-06T04:47:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:09Z- handleDvl() - RDI_Pathfinder msg: 1730868429.11 sec; seqNo:640; #beams:0 2024-11-06T04:47:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.10 , 0.13 2024-11-06T04:47:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868429.12 epoch sec; seqNo:1762; 4061693.14 north; 594519.63 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T04:47:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868426.71 2024-11-06T04:47:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:09Z- waiting for 1730868430.71, time is 1730868429.12 2024-11-06T04:47:09Z- AHRS_M2 msg: 1730868429.50 epoch sec; seqNo:1641 2024-11-06T04:47:09Z- AHRS_M2 msg: 0.11 phi; -0.06 theta; 1.51 psi 2024-11-06T04:47:09Z- handleDepth()- Depth_Keller msg: 1730868429.51 epoch sec; seqNo:1763; depth 50.19 2024-11-06T04:47:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868429.52 epoch sec; seqNo:1763; 4061693.52 north; 594519.63 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:47:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868426.71 2024-11-06T04:47:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:09Z- waiting for 1730868430.71, time is 1730868429.52 2024-11-06T04:47:09Z- AHRS_M2 msg: 1730868429.91 epoch sec; seqNo:1642 2024-11-06T04:47:09Z- AHRS_M2 msg: 0.11 phi; -0.06 theta; 1.51 psi 2024-11-06T04:47:09Z- handleDepth()- Depth_Keller msg: 1730868429.91 epoch sec; seqNo:1764; depth 50.22 2024-11-06T04:47:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868429.93 epoch sec; seqNo:1764; 4061693.52 north; 594519.63 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T04:47:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868426.71 2024-11-06T04:47:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:10Z- waiting for 1730868430.71, time is 1730868429.93 2024-11-06T04:47:10Z- AHRS_M2 msg: 1730868430.31 epoch sec; seqNo:1643 2024-11-06T04:47:10Z- AHRS_M2 msg: 0.11 phi; -0.06 theta; 1.51 psi 2024-11-06T04:47:10Z- handleDepth()- Depth_Keller msg: 1730868430.33 epoch sec; seqNo:1765; depth 50.24 2024-11-06T04:47:10Z- handleDVL() - DVL msg epochMillisec: 1730868430.35 2024-11-06T04:47:10Z- handleDvl() - RDI_Pathfinder msg: 1730868430.35 sec; seqNo:641; #beams:4 2024-11-06T04:47:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.25 , 33.30 , 35.00 , 36.02 2024-11-06T04:47:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , 0.19 2024-11-06T04:47:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868430.37 epoch sec; seqNo:1765; 4061693.52 north; 594519.63 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:47:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868426.71 2024-11-06T04:47:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:10Z- waiting for 1730868430.71, time is 1730868430.37 2024-11-06T04:47:10Z- AHRS_M2 msg: 1730868430.71 epoch sec; seqNo:1644 2024-11-06T04:47:10Z- AHRS_M2 msg: 0.11 phi; -0.05 theta; 1.51 psi 2024-11-06T04:47:10Z- handleDepth()- Depth_Keller msg: 1730868430.72 epoch sec; seqNo:1766; depth 50.29 2024-11-06T04:47:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868430.73 epoch sec; seqNo:1766; 4061693.54 north; 594520.85 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:47:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868426.71 2024-11-06T04:47:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:10Z- waiting for 1730868430.71, time is 1730868430.73 2024-11-06T04:47:10Z- LcmTrn::updateTrn() >>>> pose time 1730868430.73, meas time 1730868430.35 <<<< 2024-11-06T04:47:10Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868430.73 2024-11-06T04:47:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868430.35 sec, ping # 0; waiting for INS data more recent than 1730868426.71... 2024-11-06T04:47:10Z- TerrainNav::motionUpdate() - currEstimate = (4061694, 594520.85, 50.29) 2024-11-06T04:47:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:47:10Z- TerrainNav:: SubcloudNIS is 0.52, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:47:10Z- TNavPF::Projecting Measurements 2024-11-06T04:47:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:47:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:47:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:47:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:47:10Z- TNavFilter::4 good beams. 2024-11-06T04:47:10Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:47:10Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:47:10Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:47:10Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:47:10Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:47:10Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:47:10Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:47:10Z- TerrainMapOctree: (vehN, vehE ) = (4061693.50, 594520.51). 2024-11-06T04:47:10Z- TerrainMapOctree: Min Distance = 236.35. 2024-11-06T04:47:10Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:47:11Z- Forcing Subcloud Comparison 2024-11-06T04:47:11Z- Weighting particles with subcloud comparison 2024-11-06T04:47:11Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:11Z- meanExpectedMeasDiff: 1.565162 delta_rms_squared: 1.952650 alpha: 0.989861 2024-11-06T04:47:11Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:11Z- meanExpectedMeasDiff: 0.323027 delta_rms_squared: 1.377871 alpha: 0.985693 2024-11-06T04:47:11Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:11Z- meanExpectedMeasDiff: 0.934971 delta_rms_squared: 1.470963 alpha: 0.986586 2024-11-06T04:47:11Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:11Z- meanExpectedMeasDiff: -0.312223 delta_rms_squared: 1.674555 alpha: 0.988197 2024-11-06T04:47:11Z- Subcloud NIS: 0.505113 2024-11-06T04:47:11Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:47:11Z- TNavPF:: sumWeights = 0.443394 2024-11-06T04:47:11Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:47:11Z- TNavPF::First Term of Map Covariance Matrix is 0.51 2024-11-06T04:47:11Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T04:47:11Z- TerrainNav::Resampling particle filter... 2024-11-06T04:47:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868430.35 sec. 2024-11-06T04:47:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:47:11Z- variances([0], [2], and psi_berg) = 0.345986 0.508626 0.000000 2024-11-06T04:47:11Z- publishEstimatesDecoder() - at 1730868430.73 sec 2024-11-06T04:47:11Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:47:11Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061637.62 594524.55 56.94 2024-11-06T04:47:11Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061637.32 594525.26 56.20 lat/lon: 36.70 -121.94 2024-11-06T04:47:11Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.35 0.51 0.68 0.00 2024-11-06T04:47:11Z- AHRS_M2 msg: 1730868431.12 epoch sec; seqNo:1645 2024-11-06T04:47:11Z- AHRS_M2 msg: 0.12 phi; -0.05 theta; 1.52 psi 2024-11-06T04:47:11Z- handleDepth()- Depth_Keller msg: 1730868431.13 epoch sec; seqNo:1767; depth 50.30 2024-11-06T04:47:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868431.16 epoch sec; seqNo:1767; 4061693.54 north; 594520.85 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:47:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868430.73 2024-11-06T04:47:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:11Z- waiting for 1730868434.73, time is 1730868431.16 2024-11-06T04:47:11Z- AHRS_M2 msg: 1730868431.52 epoch sec; seqNo:1646 2024-11-06T04:47:11Z- AHRS_M2 msg: 0.12 phi; -0.05 theta; 1.52 psi 2024-11-06T04:47:11Z- handleDepth()- Depth_Keller msg: 1730868431.53 epoch sec; seqNo:1768; depth 50.32 2024-11-06T04:47:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868431.54 epoch sec; seqNo:1768; 4061693.54 north; 594520.85 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:47:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868430.73 2024-11-06T04:47:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:11Z- waiting for 1730868434.73, time is 1730868431.54 2024-11-06T04:47:11Z- handleDepth()- Depth_Keller msg: 1730868431.95 epoch sec; seqNo:1769; depth 50.34 2024-11-06T04:47:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868431.97 epoch sec; seqNo:1769; 4061693.55 north; 594522.07 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:47:12Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:47:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868430.73 2024-11-06T04:47:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:12Z- waiting for 1730868434.73, time is 1730868431.97 2024-11-06T04:47:12Z- AHRS_M2 msg: 1730868432.34 epoch sec; seqNo:1647 2024-11-06T04:47:12Z- AHRS_M2 msg: 0.13 phi; -0.05 theta; 1.53 psi 2024-11-06T04:47:12Z- handleDepth()- Depth_Keller msg: 1730868432.35 epoch sec; seqNo:1770; depth 50.36 2024-11-06T04:47:12Z- handleDVL() - DVL msg epochMillisec: 1730868432.38 2024-11-06T04:47:12Z- handleDvl() - RDI_Pathfinder msg: 1730868432.38 sec; seqNo:642; #beams:4 2024-11-06T04:47:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.76 , 33.13 , 35.51 , 36.53 2024-11-06T04:47:12Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.07 , 0.07 2024-11-06T04:47:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868432.40 epoch sec; seqNo:1770; 4061693.55 north; 594522.07 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:47:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868430.73 2024-11-06T04:47:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:12Z- waiting for 1730868434.73, time is 1730868432.40 2024-11-06T04:47:12Z- AHRS_M2 msg: 1730868432.74 epoch sec; seqNo:1648 2024-11-06T04:47:12Z- AHRS_M2 msg: 0.13 phi; -0.05 theta; 1.54 psi 2024-11-06T04:47:12Z- handleDepth()- Depth_Keller msg: 1730868432.74 epoch sec; seqNo:1771; depth 50.36 2024-11-06T04:47:12Z- handleDVL() - DVL msg epochMillisec: 1730868432.75 2024-11-06T04:47:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:12Z- handleDvl() - RDI_Pathfinder msg: 1730868432.75 sec; seqNo:643; #beams:0 2024-11-06T04:47:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:12Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.07 , 0.07 2024-11-06T04:47:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868432.77 epoch sec; seqNo:1771; 4061693.55 north; 594522.07 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:47:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868430.73 2024-11-06T04:47:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:12Z- waiting for 1730868434.73, time is 1730868432.77 2024-11-06T04:47:13Z- AHRS_M2 msg: 1730868433.14 epoch sec; seqNo:1649 2024-11-06T04:47:13Z- AHRS_M2 msg: 0.13 phi; -0.05 theta; 1.54 psi 2024-11-06T04:47:13Z- handleDepth()- Depth_Keller msg: 1730868433.15 epoch sec; seqNo:1772; depth 50.37 2024-11-06T04:47:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868433.16 epoch sec; seqNo:1772; 4061693.56 north; 594523.29 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:47:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868430.73 2024-11-06T04:47:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:13Z- waiting for 1730868434.73, time is 1730868433.16 2024-11-06T04:47:13Z- AHRS_M2 msg: 1730868433.54 epoch sec; seqNo:1650 2024-11-06T04:47:13Z- AHRS_M2 msg: 0.13 phi; -0.05 theta; 1.54 psi 2024-11-06T04:47:13Z- handleDepth()- Depth_Keller msg: 1730868433.55 epoch sec; seqNo:1773; depth 50.38 2024-11-06T04:47:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868433.56 epoch sec; seqNo:1773; 4061693.56 north; 594523.29 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:47:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868430.73 2024-11-06T04:47:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:13Z- waiting for 1730868434.73, time is 1730868433.56 2024-11-06T04:47:13Z- AHRS_M2 msg: 1730868433.95 epoch sec; seqNo:1651 2024-11-06T04:47:13Z- AHRS_M2 msg: 0.13 phi; -0.05 theta; 1.55 psi 2024-11-06T04:47:13Z- handleDepth()- Depth_Keller msg: 1730868433.95 epoch sec; seqNo:1774; depth 50.39 2024-11-06T04:47:13Z- handleDVL() - DVL msg epochMillisec: 1730868433.96 2024-11-06T04:47:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:13Z- handleDvl() - RDI_Pathfinder msg: 1730868433.96 sec; seqNo:644; #beams:0 2024-11-06T04:47:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:13Z- handleDvl() - RDI_Pathfinder msg: velocities 1.05 , 0.07 , 0.07 2024-11-06T04:47:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868433.96 epoch sec; seqNo:1774; 4061693.56 north; 594523.29 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:47:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868430.73 2024-11-06T04:47:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:14Z- waiting for 1730868434.73, time is 1730868433.96 2024-11-06T04:47:14Z- AHRS_M2 msg: 1730868434.36 epoch sec; seqNo:1652 2024-11-06T04:47:14Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.55 psi 2024-11-06T04:47:14Z- handleDepth()- Depth_Keller msg: 1730868434.37 epoch sec; seqNo:1775; depth 50.38 2024-11-06T04:47:14Z- handleDVL() - DVL msg epochMillisec: 1730868434.39 2024-11-06T04:47:14Z- handleDvl() - RDI_Pathfinder msg: 1730868434.39 sec; seqNo:645; #beams:4 2024-11-06T04:47:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.76 , 32.79 , 35.51 , 36.19 2024-11-06T04:47:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.05 , -0.09 2024-11-06T04:47:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868434.41 epoch sec; seqNo:1775; 4061693.58 north; 594524.51 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:47:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868430.73 2024-11-06T04:47:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:14Z- waiting for 1730868434.73, time is 1730868434.41 2024-11-06T04:47:14Z- AHRS_M2 msg: 1730868434.76 epoch sec; seqNo:1653 2024-11-06T04:47:14Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.55 psi 2024-11-06T04:47:14Z- handleDepth()- Depth_Keller msg: 1730868434.76 epoch sec; seqNo:1776; depth 50.38 2024-11-06T04:47:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868434.78 epoch sec; seqNo:1776; 4061693.58 north; 594524.51 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:47:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868430.73 2024-11-06T04:47:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:14Z- waiting for 1730868434.73, time is 1730868434.78 2024-11-06T04:47:14Z- LcmTrn::updateTrn() >>>> pose time 1730868434.78, meas time 1730868434.39 <<<< 2024-11-06T04:47:14Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868434.78 2024-11-06T04:47:14Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868434.39 sec, ping # 0; waiting for INS data more recent than 1730868430.73... 2024-11-06T04:47:14Z- TerrainNav::motionUpdate() - currEstimate = (4061694, 594524.51, 50.38) 2024-11-06T04:47:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:47:14Z- TerrainNav:: SubcloudNIS is 0.51, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:47:14Z- TNavPF::Projecting Measurements 2024-11-06T04:47:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:47:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:47:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:47:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:47:14Z- TNavFilter::4 good beams. 2024-11-06T04:47:14Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:47:14Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:47:14Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:47:14Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:47:14Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:47:14Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:47:14Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:47:14Z- TerrainMapOctree: (vehN, vehE ) = (4061693.57, 594524.16). 2024-11-06T04:47:14Z- TerrainMapOctree: Min Distance = 232.78. 2024-11-06T04:47:14Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:47:15Z- Forcing Subcloud Comparison 2024-11-06T04:47:15Z- Weighting particles with subcloud comparison 2024-11-06T04:47:15Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:15Z- meanExpectedMeasDiff: 1.257217 delta_rms_squared: 1.460194 alpha: 0.986488 2024-11-06T04:47:15Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:15Z- meanExpectedMeasDiff: -0.107020 delta_rms_squared: 1.063499 alpha: 0.981541 2024-11-06T04:47:15Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:15Z- meanExpectedMeasDiff: 1.010607 delta_rms_squared: 1.170479 alpha: 0.983200 2024-11-06T04:47:15Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:15Z- meanExpectedMeasDiff: -0.328313 delta_rms_squared: 1.312686 alpha: 0.984993 2024-11-06T04:47:15Z- Subcloud NIS: 0.482648 2024-11-06T04:47:15Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:47:15Z- TNavPF:: sumWeights = 0.502842 2024-11-06T04:47:15Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:47:15Z- TNavPF::First Term of Map Covariance Matrix is 0.44 2024-11-06T04:47:15Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T04:47:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868434.39 sec. 2024-11-06T04:47:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:47:15Z- variances([0], [2], and psi_berg) = 0.355843 0.507344 0.000000 2024-11-06T04:47:15Z- publishEstimatesDecoder() - at 1730868434.78 sec 2024-11-06T04:47:15Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:47:15Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061638.08 594527.94 55.68 2024-11-06T04:47:15Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061637.36 594528.93 56.08 lat/lon: 36.70 -121.94 2024-11-06T04:47:15Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.36 0.51 0.63 0.00 2024-11-06T04:47:15Z- AHRS_M2 msg: 1730868435.16 epoch sec; seqNo:1654 2024-11-06T04:47:15Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.55 psi 2024-11-06T04:47:15Z- handleDepth()- Depth_Keller msg: 1730868435.16 epoch sec; seqNo:1777; depth 50.36 2024-11-06T04:47:15Z- handleDVL() - DVL msg epochMillisec: 1730868435.17 2024-11-06T04:47:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:15Z- handleDvl() - RDI_Pathfinder msg: 1730868435.17 sec; seqNo:646; #beams:0 2024-11-06T04:47:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , 0.05 , -0.09 2024-11-06T04:47:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868435.18 epoch sec; seqNo:1777; 4061693.58 north; 594524.51 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:47:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868434.78 2024-11-06T04:47:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:15Z- waiting for 1730868438.78, time is 1730868435.18 2024-11-06T04:47:15Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:47:15Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:47:15Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:47:15Z- AHRS_M2 msg: 1730868435.56 epoch sec; seqNo:1655 2024-11-06T04:47:15Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.55 psi 2024-11-06T04:47:15Z- handleDepth()- Depth_Keller msg: 1730868435.59 epoch sec; seqNo:1778; depth 50.34 2024-11-06T04:47:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868435.61 epoch sec; seqNo:1778; 4061693.59 north; 594525.73 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:47:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868434.78 2024-11-06T04:47:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:15Z- waiting for 1730868438.78, time is 1730868435.61 2024-11-06T04:47:15Z- AHRS_M2 msg: 1730868435.97 epoch sec; seqNo:1656 2024-11-06T04:47:15Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.55 psi 2024-11-06T04:47:15Z- handleDepth()- Depth_Keller msg: 1730868435.97 epoch sec; seqNo:1779; depth 50.34 2024-11-06T04:47:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868436.00 epoch sec; seqNo:1779; 4061693.59 north; 594525.73 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:47:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868434.78 2024-11-06T04:47:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:16Z- waiting for 1730868438.78, time is 1730868436.00 2024-11-06T04:47:16Z- AHRS_M2 msg: 1730868436.37 epoch sec; seqNo:1657 2024-11-06T04:47:16Z- AHRS_M2 msg: 0.13 phi; -0.05 theta; 1.55 psi 2024-11-06T04:47:16Z- handleDepth()- Depth_Keller msg: 1730868436.38 epoch sec; seqNo:1780; depth 50.34 2024-11-06T04:47:16Z- handleDVL() - DVL msg epochMillisec: 1730868436.41 2024-11-06T04:47:16Z- handleDvl() - RDI_Pathfinder msg: 1730868436.41 sec; seqNo:647; #beams:4 2024-11-06T04:47:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.76 , 32.45 , 35.51 , 36.02 2024-11-06T04:47:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , -0.13 2024-11-06T04:47:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868436.42 epoch sec; seqNo:1780; 4061693.59 north; 594525.73 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:47:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868434.78 2024-11-06T04:47:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:16Z- waiting for 1730868438.78, time is 1730868436.42 2024-11-06T04:47:16Z- AHRS_M2 msg: 1730868436.78 epoch sec; seqNo:1658 2024-11-06T04:47:16Z- AHRS_M2 msg: 0.13 phi; -0.05 theta; 1.55 psi 2024-11-06T04:47:16Z- handleDepth()- Depth_Keller msg: 1730868436.78 epoch sec; seqNo:1781; depth 50.33 2024-11-06T04:47:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868436.80 epoch sec; seqNo:1781; 4061693.60 north; 594526.95 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T04:47:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868434.78 2024-11-06T04:47:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:16Z- waiting for 1730868438.78, time is 1730868436.80 2024-11-06T04:47:17Z- AHRS_M2 msg: 1730868437.18 epoch sec; seqNo:1659 2024-11-06T04:47:17Z- AHRS_M2 msg: 0.13 phi; -0.05 theta; 1.54 psi 2024-11-06T04:47:17Z- handleDepth()- Depth_Keller msg: 1730868437.19 epoch sec; seqNo:1782; depth 50.31 2024-11-06T04:47:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868437.20 epoch sec; seqNo:1782; 4061693.60 north; 594526.95 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:47:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868434.78 2024-11-06T04:47:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:17Z- waiting for 1730868438.78, time is 1730868437.20 2024-11-06T04:47:17Z- AHRS_M2 msg: 1730868437.58 epoch sec; seqNo:1660 2024-11-06T04:47:17Z- AHRS_M2 msg: 0.13 phi; -0.05 theta; 1.54 psi 2024-11-06T04:47:17Z- handleDepth()- Depth_Keller msg: 1730868437.59 epoch sec; seqNo:1783; depth 50.30 2024-11-06T04:47:17Z- handleDVL() - DVL msg epochMillisec: 1730868437.59 2024-11-06T04:47:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:17Z- handleDvl() - RDI_Pathfinder msg: 1730868437.59 sec; seqNo:648; #beams:0 2024-11-06T04:47:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , -0.13 2024-11-06T04:47:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868437.60 epoch sec; seqNo:1783; 4061693.60 north; 594526.95 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:47:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868434.78 2024-11-06T04:47:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:17Z- waiting for 1730868438.78, time is 1730868437.60 2024-11-06T04:47:17Z- AHRS_M2 msg: 1730868437.99 epoch sec; seqNo:1661 2024-11-06T04:47:17Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 1.54 psi 2024-11-06T04:47:17Z- handleDepth()- Depth_Keller msg: 1730868437.99 epoch sec; seqNo:1784; depth 50.29 2024-11-06T04:47:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868438.01 epoch sec; seqNo:1784; 4061693.60 north; 594526.95 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:47:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868434.78 2024-11-06T04:47:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:18Z- waiting for 1730868438.78, time is 1730868438.01 2024-11-06T04:47:18Z- AHRS_M2 msg: 1730868438.39 epoch sec; seqNo:1662 2024-11-06T04:47:18Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 1.53 psi 2024-11-06T04:47:18Z- handleDepth()- Depth_Keller msg: 1730868438.39 epoch sec; seqNo:1785; depth 50.30 2024-11-06T04:47:18Z- handleDVL() - DVL msg epochMillisec: 1730868438.43 2024-11-06T04:47:18Z- handleDvl() - RDI_Pathfinder msg: 1730868438.43 sec; seqNo:649; #beams:4 2024-11-06T04:47:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.25 , 32.79 , 35.17 , 36.02 2024-11-06T04:47:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.09 , -0.07 2024-11-06T04:47:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868438.44 epoch sec; seqNo:1785; 4061693.62 north; 594528.17 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:47:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868434.78 2024-11-06T04:47:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:18Z- waiting for 1730868438.78, time is 1730868438.44 2024-11-06T04:47:18Z- AHRS_M2 msg: 1730868438.80 epoch sec; seqNo:1663 2024-11-06T04:47:18Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 1.53 psi 2024-11-06T04:47:18Z- handleDepth()- Depth_Keller msg: 1730868438.81 epoch sec; seqNo:1786; depth 50.28 2024-11-06T04:47:18Z- handleDVL() - DVL msg epochMillisec: 1730868438.81 2024-11-06T04:47:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:18Z- handleDvl() - RDI_Pathfinder msg: 1730868438.81 sec; seqNo:650; #beams:0 2024-11-06T04:47:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.09 , -0.07 2024-11-06T04:47:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868438.84 epoch sec; seqNo:1786; 4061693.62 north; 594528.17 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T04:47:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868434.78 2024-11-06T04:47:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:18Z- waiting for 1730868438.78, time is 1730868438.84 2024-11-06T04:47:18Z- LcmTrn::updateTrn() >>>> pose time 1730868438.84, meas time 1730868438.81 <<<< 2024-11-06T04:47:18Z- LcmTrn::updateTrn() equating pose 1730868438.81 and meas times 1730868438.81 2024-11-06T04:47:18Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868438.81 2024-11-06T04:47:18Z- TerrainNav::motionUpdate() - currEstimate = (4061694, 594528.17, 50.28) 2024-11-06T04:47:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:47:18Z- TerrainNav:: SubcloudNIS is 0.48, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:47:18Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:47:18Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:47:18Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:47:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:47:18Z- variances([0], [2], and psi_berg) = 0.358106 0.509682 0.000000 2024-11-06T04:47:18Z- publishEstimatesDecoder() - at 1730868438.81 sec 2024-11-06T04:47:18Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:47:18Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061638.12 594531.60 55.34 2024-11-06T04:47:18Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061637.40 594532.59 55.98 lat/lon: 36.70 -121.94 2024-11-06T04:47:18Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.36 0.51 0.88 0.00 2024-11-06T04:47:19Z- AHRS_M2 msg: 1730868439.20 epoch sec; seqNo:1664 2024-11-06T04:47:19Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 1.52 psi 2024-11-06T04:47:19Z- handleDepth()- Depth_Keller msg: 1730868439.21 epoch sec; seqNo:1787; depth 50.29 2024-11-06T04:47:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868439.22 epoch sec; seqNo:1787; 4061693.62 north; 594528.17 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:47:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868438.81 2024-11-06T04:47:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:19Z- waiting for 1730868442.81, time is 1730868439.22 2024-11-06T04:47:19Z- AHRS_M2 msg: 1730868439.60 epoch sec; seqNo:1665 2024-11-06T04:47:19Z- AHRS_M2 msg: 0.11 phi; -0.04 theta; 1.52 psi 2024-11-06T04:47:19Z- handleDepth()- Depth_Keller msg: 1730868439.61 epoch sec; seqNo:1788; depth 50.30 2024-11-06T04:47:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868439.63 epoch sec; seqNo:1788; 4061694.00 north; 594528.17 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:47:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868438.81 2024-11-06T04:47:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:19Z- waiting for 1730868442.81, time is 1730868439.63 2024-11-06T04:47:20Z- AHRS_M2 msg: 1730868440.01 epoch sec; seqNo:1666 2024-11-06T04:47:20Z- AHRS_M2 msg: 0.11 phi; -0.04 theta; 1.52 psi 2024-11-06T04:47:20Z- handleDepth()- Depth_Keller msg: 1730868440.02 epoch sec; seqNo:1789; depth 50.31 2024-11-06T04:47:20Z- handleDVL() - DVL msg epochMillisec: 1730868440.02 2024-11-06T04:47:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:20Z- handleDvl() - RDI_Pathfinder msg: 1730868440.02 sec; seqNo:651; #beams:0 2024-11-06T04:47:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.09 , -0.07 2024-11-06T04:47:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868440.03 epoch sec; seqNo:1789; 4061694.01 north; 594529.39 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:47:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868438.81 2024-11-06T04:47:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:20Z- waiting for 1730868442.81, time is 1730868440.03 2024-11-06T04:47:20Z- AHRS_M2 msg: 1730868440.41 epoch sec; seqNo:1667 2024-11-06T04:47:20Z- AHRS_M2 msg: 0.11 phi; -0.04 theta; 1.51 psi 2024-11-06T04:47:20Z- handleDepth()- Depth_Keller msg: 1730868440.41 epoch sec; seqNo:1790; depth 50.34 2024-11-06T04:47:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868440.46 epoch sec; seqNo:1790; 4061694.01 north; 594529.39 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:47:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868438.81 2024-11-06T04:47:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:20Z- waiting for 1730868442.81, time is 1730868440.46 2024-11-06T04:47:20Z- handleDepth()- Depth_Keller msg: 1730868440.84 epoch sec; seqNo:1791; depth 50.36 2024-11-06T04:47:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868440.86 epoch sec; seqNo:1791; 4061694.01 north; 594529.39 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:47:20Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:47:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868438.81 2024-11-06T04:47:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:21Z- waiting for 1730868442.81, time is 1730868440.86 2024-11-06T04:47:21Z- AHRS_M2 msg: 1730868441.23 epoch sec; seqNo:1668 2024-11-06T04:47:21Z- AHRS_M2 msg: 0.11 phi; -0.04 theta; 1.51 psi 2024-11-06T04:47:21Z- handleDepth()- Depth_Keller msg: 1730868441.24 epoch sec; seqNo:1792; depth 50.37 2024-11-06T04:47:21Z- handleDVL() - DVL msg epochMillisec: 1730868441.24 2024-11-06T04:47:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:21Z- handleDvl() - RDI_Pathfinder msg: 1730868441.24 sec; seqNo:652; #beams:0 2024-11-06T04:47:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.70 , -0.09 , -0.07 2024-11-06T04:47:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868441.25 epoch sec; seqNo:1792; 4061694.01 north; 594529.39 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:47:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868438.81 2024-11-06T04:47:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:21Z- waiting for 1730868442.81, time is 1730868441.25 2024-11-06T04:47:21Z- AHRS_M2 msg: 1730868441.62 epoch sec; seqNo:1669 2024-11-06T04:47:21Z- AHRS_M2 msg: 0.11 phi; -0.04 theta; 1.50 psi 2024-11-06T04:47:21Z- handleDepth()- Depth_Keller msg: 1730868441.63 epoch sec; seqNo:1793; depth 50.39 2024-11-06T04:47:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868441.64 epoch sec; seqNo:1793; 4061694.01 north; 594529.39 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:47:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868438.81 2024-11-06T04:47:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:21Z- waiting for 1730868442.81, time is 1730868441.64 2024-11-06T04:47:22Z- AHRS_M2 msg: 1730868442.03 epoch sec; seqNo:1670 2024-11-06T04:47:22Z- AHRS_M2 msg: 0.11 phi; -0.04 theta; 1.50 psi 2024-11-06T04:47:22Z- handleDepth()- Depth_Keller msg: 1730868442.03 epoch sec; seqNo:1794; depth 50.42 2024-11-06T04:47:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868442.04 epoch sec; seqNo:1794; 4061694.02 north; 594530.61 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T04:47:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868438.81 2024-11-06T04:47:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:22Z- waiting for 1730868442.81, time is 1730868442.04 2024-11-06T04:47:22Z- AHRS_M2 msg: 1730868442.43 epoch sec; seqNo:1671 2024-11-06T04:47:22Z- AHRS_M2 msg: 0.11 phi; -0.04 theta; 1.50 psi 2024-11-06T04:47:22Z- handleDepth()- Depth_Keller msg: 1730868442.44 epoch sec; seqNo:1795; depth 50.45 2024-11-06T04:47:22Z- handleDVL() - DVL msg epochMillisec: 1730868442.46 2024-11-06T04:47:22Z- handleDvl() - RDI_Pathfinder msg: 1730868442.46 sec; seqNo:653; #beams:4 2024-11-06T04:47:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.74 , 33.64 , 35.00 , 36.19 2024-11-06T04:47:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.05 , 0.09 2024-11-06T04:47:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868442.50 epoch sec; seqNo:1795; 4061694.02 north; 594530.61 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T04:47:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868438.81 2024-11-06T04:47:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:22Z- waiting for 1730868442.81, time is 1730868442.50 2024-11-06T04:47:22Z- AHRS_M2 msg: 1730868442.84 epoch sec; seqNo:1672 2024-11-06T04:47:22Z- AHRS_M2 msg: 0.11 phi; -0.04 theta; 1.50 psi 2024-11-06T04:47:22Z- handleDepth()- Depth_Keller msg: 1730868442.85 epoch sec; seqNo:1796; depth 50.48 2024-11-06T04:47:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868442.86 epoch sec; seqNo:1796; 4061694.40 north; 594530.60 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T04:47:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868438.81 2024-11-06T04:47:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:23Z- waiting for 1730868442.81, time is 1730868442.86 2024-11-06T04:47:23Z- LcmTrn::updateTrn() >>>> pose time 1730868442.86, meas time 1730868442.46 <<<< 2024-11-06T04:47:23Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868442.86 2024-11-06T04:47:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868442.46 sec, ping # 0; waiting for INS data more recent than 1730868438.81... 2024-11-06T04:47:23Z- TerrainNav::motionUpdate() - currEstimate = (4061694, 594530.60, 50.48) 2024-11-06T04:47:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:47:23Z- TerrainNav:: SubcloudNIS is 0.48, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:47:23Z- TNavPF::Projecting Measurements 2024-11-06T04:47:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:47:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:47:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:47:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:47:23Z- TNavFilter::4 good beams. 2024-11-06T04:47:23Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:47:23Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:47:23Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:47:23Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:47:23Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:47:23Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:47:23Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:47:23Z- TerrainMapOctree: (vehN, vehE ) = (4061694.32, 594530.36). 2024-11-06T04:47:23Z- TerrainMapOctree: Min Distance = 226.58. 2024-11-06T04:47:23Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:47:23Z- Forcing Subcloud Comparison 2024-11-06T04:47:23Z- Weighting particles with subcloud comparison 2024-11-06T04:47:23Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:23Z- meanExpectedMeasDiff: 0.772290 delta_rms_squared: 1.754282 alpha: 0.988728 2024-11-06T04:47:23Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:23Z- meanExpectedMeasDiff: 0.311977 delta_rms_squared: 1.349660 alpha: 0.985398 2024-11-06T04:47:23Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:23Z- meanExpectedMeasDiff: 0.241528 delta_rms_squared: 1.468875 alpha: 0.986567 2024-11-06T04:47:23Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:23Z- meanExpectedMeasDiff: -0.075652 delta_rms_squared: 1.599815 alpha: 0.987653 2024-11-06T04:47:23Z- Subcloud NIS: 0.457408 2024-11-06T04:47:23Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:47:23Z- TNavPF:: sumWeights = 0.585535 2024-11-06T04:47:23Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:47:23Z- TNavPF::First Term of Map Covariance Matrix is 0.42 2024-11-06T04:47:23Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.01 2024-11-06T04:47:23Z- TerrainNav::Resampling particle filter... 2024-11-06T04:47:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868442.46 sec. 2024-11-06T04:47:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:47:23Z- variances([0], [2], and psi_berg) = 0.363164 0.515683 0.000000 2024-11-06T04:47:23Z- publishEstimatesDecoder() - at 1730868442.86 sec 2024-11-06T04:47:23Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:47:23Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061638.13 594534.56 55.51 2024-11-06T04:47:23Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061638.19 594535.01 56.02 lat/lon: 36.70 -121.94 2024-11-06T04:47:23Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.36 0.52 0.69 0.00 2024-11-06T04:47:23Z- AHRS_M2 msg: 1730868443.24 epoch sec; seqNo:1673 2024-11-06T04:47:23Z- AHRS_M2 msg: 0.11 phi; -0.04 theta; 1.50 psi 2024-11-06T04:47:23Z- handleDepth()- Depth_Keller msg: 1730868443.26 epoch sec; seqNo:1797; depth 50.51 2024-11-06T04:47:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868443.28 epoch sec; seqNo:1797; 4061694.40 north; 594530.60 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T04:47:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868442.86 2024-11-06T04:47:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:23Z- waiting for 1730868446.86, time is 1730868443.28 2024-11-06T04:47:23Z- AHRS_M2 msg: 1730868443.64 epoch sec; seqNo:1674 2024-11-06T04:47:23Z- AHRS_M2 msg: 0.11 phi; -0.03 theta; 1.50 psi 2024-11-06T04:47:23Z- handleDepth()- Depth_Keller msg: 1730868443.65 epoch sec; seqNo:1798; depth 50.53 2024-11-06T04:47:23Z- handleDVL() - DVL msg epochMillisec: 1730868443.66 2024-11-06T04:47:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:23Z- handleDvl() - RDI_Pathfinder msg: 1730868443.66 sec; seqNo:654; #beams:0 2024-11-06T04:47:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.05 , 0.09 2024-11-06T04:47:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868443.67 epoch sec; seqNo:1798; 4061694.42 north; 594531.82 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T04:47:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868442.86 2024-11-06T04:47:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:23Z- waiting for 1730868446.86, time is 1730868443.67 2024-11-06T04:47:24Z- AHRS_M2 msg: 1730868444.05 epoch sec; seqNo:1675 2024-11-06T04:47:24Z- AHRS_M2 msg: 0.11 phi; -0.03 theta; 1.50 psi 2024-11-06T04:47:24Z- handleDepth()- Depth_Keller msg: 1730868444.05 epoch sec; seqNo:1799; depth 50.55 2024-11-06T04:47:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868444.06 epoch sec; seqNo:1799; 4061694.42 north; 594531.82 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:47:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868442.86 2024-11-06T04:47:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:24Z- waiting for 1730868446.86, time is 1730868444.06 2024-11-06T04:47:24Z- handleDepth()- Depth_Keller msg: 1730868444.47 epoch sec; seqNo:1800; depth 50.57 2024-11-06T04:47:24Z- handleDVL() - DVL msg epochMillisec: 1730868444.49 2024-11-06T04:47:24Z- handleDvl() - RDI_Pathfinder msg: 1730868444.49 sec; seqNo:655; #beams:4 2024-11-06T04:47:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.91 , 33.64 , 35.34 , 36.53 2024-11-06T04:47:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , 0.10 2024-11-06T04:47:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868444.53 epoch sec; seqNo:1800; 4061694.42 north; 594531.82 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T04:47:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868442.86 2024-11-06T04:47:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:24Z- waiting for 1730868446.86, time is 1730868444.53 2024-11-06T04:47:24Z- AHRS_M2 msg: 1730868444.86 epoch sec; seqNo:1676 2024-11-06T04:47:24Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.50 psi 2024-11-06T04:47:24Z- handleDepth()- Depth_Keller msg: 1730868444.87 epoch sec; seqNo:1801; depth 50.59 2024-11-06T04:47:24Z- handleDVL() - DVL msg epochMillisec: 1730868444.87 2024-11-06T04:47:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:24Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:24Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:24Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:24Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:24Z- handleDvl() - RDI_Pathfinder msg: 1730868444.87 sec; seqNo:656; #beams:0 2024-11-06T04:47:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , 0.10 2024-11-06T04:47:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868444.89 epoch sec; seqNo:1801; 4061694.43 north; 594533.04 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T04:47:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868442.86 2024-11-06T04:47:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:25Z- waiting for 1730868446.86, time is 1730868444.89 2024-11-06T04:47:25Z- AHRS_M2 msg: 1730868445.26 epoch sec; seqNo:1677 2024-11-06T04:47:25Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.50 psi 2024-11-06T04:47:25Z- handleDepth()- Depth_Keller msg: 1730868445.26 epoch sec; seqNo:1802; depth 50.60 2024-11-06T04:47:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868445.28 epoch sec; seqNo:1802; 4061694.43 north; 594533.04 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T04:47:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868442.86 2024-11-06T04:47:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:25Z- waiting for 1730868446.86, time is 1730868445.28 2024-11-06T04:47:25Z- AHRS_M2 msg: 1730868445.68 epoch sec; seqNo:1678 2024-11-06T04:47:25Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.50 psi 2024-11-06T04:47:25Z- handleDepth()- Depth_Keller msg: 1730868445.68 epoch sec; seqNo:1803; depth 50.63 2024-11-06T04:47:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868445.69 epoch sec; seqNo:1803; 4061694.43 north; 594533.04 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T04:47:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868442.86 2024-11-06T04:47:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:25Z- waiting for 1730868446.86, time is 1730868445.69 2024-11-06T04:47:26Z- AHRS_M2 msg: 1730868446.08 epoch sec; seqNo:1679 2024-11-06T04:47:26Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.50 psi 2024-11-06T04:47:26Z- handleDepth()- Depth_Keller msg: 1730868446.10 epoch sec; seqNo:1804; depth 50.65 2024-11-06T04:47:26Z- handleDVL() - DVL msg epochMillisec: 1730868446.11 2024-11-06T04:47:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:26Z- handleDvl() - RDI_Pathfinder msg: 1730868446.11 sec; seqNo:657; #beams:0 2024-11-06T04:47:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , 0.10 2024-11-06T04:47:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868446.15 epoch sec; seqNo:1804; 4061694.43 north; 594533.04 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T04:47:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868442.86 2024-11-06T04:47:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:26Z- waiting for 1730868446.86, time is 1730868446.15 2024-11-06T04:47:26Z- handleDepth()- Depth_Keller msg: 1730868446.49 epoch sec; seqNo:1805; depth 50.63 2024-11-06T04:47:26Z- handleDVL() - DVL msg epochMillisec: 1730868446.51 2024-11-06T04:47:26Z- handleDvl() - RDI_Pathfinder msg: 1730868446.51 sec; seqNo:658; #beams:4 2024-11-06T04:47:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.42 , 33.47 , 35.68 , 36.53 2024-11-06T04:47:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , 0.04 , 0.05 2024-11-06T04:47:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868446.53 epoch sec; seqNo:1805; 4061694.44 north; 594534.26 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T04:47:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868442.86 2024-11-06T04:47:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:26Z- waiting for 1730868446.86, time is 1730868446.53 2024-11-06T04:47:26Z- AHRS_M2 msg: 1730868446.88 epoch sec; seqNo:1680 2024-11-06T04:47:26Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.50 psi 2024-11-06T04:47:26Z- handleDepth()- Depth_Keller msg: 1730868446.89 epoch sec; seqNo:1806; depth 50.65 2024-11-06T04:47:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868446.91 epoch sec; seqNo:1806; 4061694.44 north; 594534.26 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T04:47:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868442.86 2024-11-06T04:47:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:27Z- waiting for 1730868446.86, time is 1730868446.91 2024-11-06T04:47:27Z- LcmTrn::updateTrn() >>>> pose time 1730868446.91, meas time 1730868446.51 <<<< 2024-11-06T04:47:27Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868446.91 2024-11-06T04:47:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868446.51 sec, ping # 0; waiting for INS data more recent than 1730868442.86... 2024-11-06T04:47:27Z- TerrainNav::motionUpdate() - currEstimate = (4061694, 594534.26, 50.65) 2024-11-06T04:47:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:47:27Z- TerrainNav:: SubcloudNIS is 0.46, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:47:27Z- TNavPF::Projecting Measurements 2024-11-06T04:47:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:47:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:47:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:47:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:47:27Z- TNavFilter::4 good beams. 2024-11-06T04:47:27Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:47:27Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:47:27Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:47:27Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:47:27Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:47:27Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:47:27Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:47:27Z- TerrainMapOctree: (vehN, vehE ) = (4061694.44, 594533.91). 2024-11-06T04:47:27Z- TerrainMapOctree: Min Distance = 223.12. 2024-11-06T04:47:27Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:47:27Z- Forcing Subcloud Comparison 2024-11-06T04:47:27Z- Weighting particles with subcloud comparison 2024-11-06T04:47:27Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:27Z- meanExpectedMeasDiff: 1.166670 delta_rms_squared: 1.469150 alpha: 0.986570 2024-11-06T04:47:27Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:27Z- meanExpectedMeasDiff: 0.337038 delta_rms_squared: 1.099855 alpha: 0.982141 2024-11-06T04:47:27Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:27Z- meanExpectedMeasDiff: 0.888904 delta_rms_squared: 1.123355 alpha: 0.982508 2024-11-06T04:47:27Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:27Z- meanExpectedMeasDiff: 0.322441 delta_rms_squared: 1.315311 alpha: 0.985022 2024-11-06T04:47:27Z- Subcloud NIS: 0.429004 2024-11-06T04:47:27Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:47:27Z- TNavPF:: sumWeights = 0.561715 2024-11-06T04:47:27Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:47:27Z- TNavPF::First Term of Map Covariance Matrix is 0.43 2024-11-06T04:47:27Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.02 2024-11-06T04:47:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868446.51 sec. 2024-11-06T04:47:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:47:27Z- variances([0], [2], and psi_berg) = 0.369907 0.517241 0.000000 2024-11-06T04:47:27Z- publishEstimatesDecoder() - at 1730868446.91 sec 2024-11-06T04:47:27Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:47:27Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061639.09 594538.50 55.62 2024-11-06T04:47:27Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061638.23 594538.67 55.88 lat/lon: 36.70 -121.94 2024-11-06T04:47:27Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.37 0.52 0.67 0.00 2024-11-06T04:47:27Z- AHRS_M2 msg: 1730868447.29 epoch sec; seqNo:1681 2024-11-06T04:47:27Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.51 psi 2024-11-06T04:47:27Z- handleDepth()- Depth_Keller msg: 1730868447.31 epoch sec; seqNo:1807; depth 50.66 2024-11-06T04:47:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868447.33 epoch sec; seqNo:1807; 4061694.44 north; 594534.26 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T04:47:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868446.91 2024-11-06T04:47:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:27Z- waiting for 1730868450.91, time is 1730868447.33 2024-11-06T04:47:27Z- handleDepth()- Depth_Keller msg: 1730868447.71 epoch sec; seqNo:1808; depth 50.64 2024-11-06T04:47:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868447.72 epoch sec; seqNo:1808; 4061694.46 north; 594535.48 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T04:47:27Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:47:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868446.91 2024-11-06T04:47:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:27Z- waiting for 1730868450.91, time is 1730868447.72 2024-11-06T04:47:28Z- AHRS_M2 msg: 1730868448.14 epoch sec; seqNo:1682 2024-11-06T04:47:28Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.51 psi 2024-11-06T04:47:28Z- handleDepth()- Depth_Keller msg: 1730868448.16 epoch sec; seqNo:1809; depth 50.65 2024-11-06T04:47:28Z- handleDVL() - DVL msg epochMillisec: 1730868448.18 2024-11-06T04:47:28Z- handleDvl() - RDI_Pathfinder msg: 1730868448.18 sec; seqNo:659; #beams:4 2024-11-06T04:47:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.59 , 33.47 , 35.51 , 36.36 2024-11-06T04:47:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.03 , -0.03 2024-11-06T04:47:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868448.20 epoch sec; seqNo:1809; 4061694.46 north; 594535.48 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T04:47:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868446.91 2024-11-06T04:47:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:28Z- waiting for 1730868450.91, time is 1730868448.20 2024-11-06T04:47:28Z- handleDepth()- Depth_Keller msg: 1730868448.51 epoch sec; seqNo:1810; depth 50.65 2024-11-06T04:47:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868448.54 epoch sec; seqNo:1810; 4061694.46 north; 594535.48 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T04:47:28Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:47:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868446.91 2024-11-06T04:47:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:28Z- waiting for 1730868450.91, time is 1730868448.54 2024-11-06T04:47:28Z- AHRS_M2 msg: 1730868448.89 epoch sec; seqNo:1683 2024-11-06T04:47:28Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.51 psi 2024-11-06T04:47:28Z- handleDepth()- Depth_Keller msg: 1730868448.91 epoch sec; seqNo:1811; depth 50.65 2024-11-06T04:47:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868448.92 epoch sec; seqNo:1811; 4061694.85 north; 594536.70 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T04:47:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868446.91 2024-11-06T04:47:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:29Z- waiting for 1730868450.91, time is 1730868448.92 2024-11-06T04:47:29Z- AHRS_M2 msg: 1730868449.30 epoch sec; seqNo:1684 2024-11-06T04:47:29Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.51 psi 2024-11-06T04:47:29Z- handleDepth()- Depth_Keller msg: 1730868449.31 epoch sec; seqNo:1812; depth 50.63 2024-11-06T04:47:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868449.32 epoch sec; seqNo:1812; 4061694.85 north; 594536.70 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T04:47:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868446.91 2024-11-06T04:47:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:29Z- waiting for 1730868450.91, time is 1730868449.32 2024-11-06T04:47:29Z- AHRS_M2 msg: 1730868449.71 epoch sec; seqNo:1685 2024-11-06T04:47:29Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.51 psi 2024-11-06T04:47:29Z- handleDepth()- Depth_Keller msg: 1730868449.73 epoch sec; seqNo:1813; depth 50.63 2024-11-06T04:47:29Z- handleDVL() - DVL msg epochMillisec: 1730868449.73 2024-11-06T04:47:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:29Z- handleDvl() - RDI_Pathfinder msg: 1730868449.73 sec; seqNo:660; #beams:0 2024-11-06T04:47:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.03 , -0.03 2024-11-06T04:47:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868449.74 epoch sec; seqNo:1813; 4061694.85 north; 594536.70 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T04:47:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868446.91 2024-11-06T04:47:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:29Z- waiting for 1730868450.91, time is 1730868449.74 2024-11-06T04:47:30Z- AHRS_M2 msg: 1730868450.15 epoch sec; seqNo:1686 2024-11-06T04:47:30Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.51 psi 2024-11-06T04:47:30Z- handleDepth()- Depth_Keller msg: 1730868450.16 epoch sec; seqNo:1814; depth 50.60 2024-11-06T04:47:30Z- handleDVL() - DVL msg epochMillisec: 1730868450.18 2024-11-06T04:47:30Z- handleDvl() - RDI_Pathfinder msg: 1730868450.18 sec; seqNo:661; #beams:4 2024-11-06T04:47:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.25 , 33.30 , 35.68 , 35.85 2024-11-06T04:47:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , -0.10 2024-11-06T04:47:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868450.21 epoch sec; seqNo:1814; 4061694.86 north; 594537.92 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T04:47:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868446.91 2024-11-06T04:47:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:30Z- waiting for 1730868450.91, time is 1730868450.21 2024-11-06T04:47:30Z- AHRS_M2 msg: 1730868450.51 epoch sec; seqNo:1687 2024-11-06T04:47:30Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.51 psi 2024-11-06T04:47:30Z- handleDepth()- Depth_Keller msg: 1730868450.52 epoch sec; seqNo:1815; depth 50.60 2024-11-06T04:47:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868450.54 epoch sec; seqNo:1815; 4061694.86 north; 594537.92 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T04:47:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868446.91 2024-11-06T04:47:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:30Z- waiting for 1730868450.91, time is 1730868450.54 2024-11-06T04:47:30Z- AHRS_M2 msg: 1730868450.91 epoch sec; seqNo:1688 2024-11-06T04:47:30Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.51 psi 2024-11-06T04:47:30Z- handleDepth()- Depth_Keller msg: 1730868450.92 epoch sec; seqNo:1816; depth 50.59 2024-11-06T04:47:30Z- handleDVL() - DVL msg epochMillisec: 1730868450.92 2024-11-06T04:47:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:30Z- handleDvl() - RDI_Pathfinder msg: 1730868450.92 sec; seqNo:662; #beams:0 2024-11-06T04:47:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , -0.10 2024-11-06T04:47:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868450.93 epoch sec; seqNo:1816; 4061694.86 north; 594537.92 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T04:47:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868446.91 2024-11-06T04:47:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:31Z- waiting for 1730868450.91, time is 1730868450.93 2024-11-06T04:47:31Z- LcmTrn::updateTrn() >>>> pose time 1730868450.93, meas time 1730868450.92 <<<< 2024-11-06T04:47:31Z- LcmTrn::updateTrn() equating pose 1730868450.92 and meas times 1730868450.92 2024-11-06T04:47:31Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868450.92 2024-11-06T04:47:31Z- TerrainNav::motionUpdate() - currEstimate = (4061695, 594537.92, 50.59) 2024-11-06T04:47:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:47:31Z- TerrainNav:: SubcloudNIS is 0.43, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:47:31Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:47:31Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:47:31Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:47:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:47:31Z- variances([0], [2], and psi_berg) = 0.370907 0.519527 0.000000 2024-11-06T04:47:31Z- publishEstimatesDecoder() - at 1730868450.92 sec 2024-11-06T04:47:31Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:47:31Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061639.48 594542.18 55.26 2024-11-06T04:47:31Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061638.65 594542.33 55.82 lat/lon: 36.70 -121.94 2024-11-06T04:47:31Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.37 0.52 0.91 0.00 2024-11-06T04:47:31Z- AHRS_M2 msg: 1730868451.32 epoch sec; seqNo:1689 2024-11-06T04:47:31Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.51 psi 2024-11-06T04:47:31Z- handleDepth()- Depth_Keller msg: 1730868451.33 epoch sec; seqNo:1817; depth 50.58 2024-11-06T04:47:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868451.34 epoch sec; seqNo:1817; 4061694.86 north; 594537.92 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T04:47:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868450.92 2024-11-06T04:47:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:31Z- waiting for 1730868454.92, time is 1730868451.34 2024-11-06T04:47:31Z- AHRS_M2 msg: 1730868451.72 epoch sec; seqNo:1690 2024-11-06T04:47:31Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.51 psi 2024-11-06T04:47:31Z- handleDepth()- Depth_Keller msg: 1730868451.73 epoch sec; seqNo:1818; depth 50.57 2024-11-06T04:47:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868451.75 epoch sec; seqNo:1818; 4061694.88 north; 594539.14 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T04:47:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868450.92 2024-11-06T04:47:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:31Z- waiting for 1730868454.92, time is 1730868451.75 2024-11-06T04:47:32Z- AHRS_M2 msg: 1730868452.16 epoch sec; seqNo:1691 2024-11-06T04:47:32Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.51 psi 2024-11-06T04:47:32Z- handleDepth()- Depth_Keller msg: 1730868452.16 epoch sec; seqNo:1819; depth 50.55 2024-11-06T04:47:32Z- handleDVL() - DVL msg epochMillisec: 1730868452.18 2024-11-06T04:47:32Z- handleDvl() - RDI_Pathfinder msg: 1730868452.18 sec; seqNo:663; #beams:4 2024-11-06T04:47:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.42 , 33.13 , 35.68 , 35.68 2024-11-06T04:47:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.08 , -0.09 2024-11-06T04:47:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868452.19 epoch sec; seqNo:1819; 4061694.88 north; 594539.14 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:47:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868450.92 2024-11-06T04:47:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:32Z- waiting for 1730868454.92, time is 1730868452.19 2024-11-06T04:47:32Z- AHRS_M2 msg: 1730868452.53 epoch sec; seqNo:1692 2024-11-06T04:47:32Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.51 psi 2024-11-06T04:47:32Z- handleDepth()- Depth_Keller msg: 1730868452.54 epoch sec; seqNo:1820; depth 50.55 2024-11-06T04:47:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868452.56 epoch sec; seqNo:1820; 4061694.88 north; 594539.14 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:47:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868450.92 2024-11-06T04:47:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:32Z- waiting for 1730868454.92, time is 1730868452.56 2024-11-06T04:47:32Z- AHRS_M2 msg: 1730868452.93 epoch sec; seqNo:1693 2024-11-06T04:47:32Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.51 psi 2024-11-06T04:47:32Z- handleDepth()- Depth_Keller msg: 1730868452.95 epoch sec; seqNo:1821; depth 50.54 2024-11-06T04:47:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868452.96 epoch sec; seqNo:1821; 4061694.89 north; 594540.36 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T04:47:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868450.92 2024-11-06T04:47:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:33Z- waiting for 1730868454.92, time is 1730868452.96 2024-11-06T04:47:33Z- AHRS_M2 msg: 1730868453.34 epoch sec; seqNo:1694 2024-11-06T04:47:33Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.51 psi 2024-11-06T04:47:33Z- handleDepth()- Depth_Keller msg: 1730868453.34 epoch sec; seqNo:1822; depth 50.54 2024-11-06T04:47:33Z- handleDVL() - DVL msg epochMillisec: 1730868453.35 2024-11-06T04:47:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:33Z- handleDvl() - RDI_Pathfinder msg: 1730868453.35 sec; seqNo:664; #beams:0 2024-11-06T04:47:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.08 , -0.09 2024-11-06T04:47:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868453.36 epoch sec; seqNo:1822; 4061694.89 north; 594540.36 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T04:47:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868450.92 2024-11-06T04:47:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:33Z- waiting for 1730868454.92, time is 1730868453.36 2024-11-06T04:47:33Z- AHRS_M2 msg: 1730868453.74 epoch sec; seqNo:1695 2024-11-06T04:47:33Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.51 psi 2024-11-06T04:47:33Z- handleDepth()- Depth_Keller msg: 1730868453.75 epoch sec; seqNo:1823; depth 50.52 2024-11-06T04:47:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868453.76 epoch sec; seqNo:1823; 4061695.27 north; 594540.36 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T04:47:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868450.92 2024-11-06T04:47:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:33Z- waiting for 1730868454.92, time is 1730868453.76 2024-11-06T04:47:34Z- AHRS_M2 msg: 1730868454.16 epoch sec; seqNo:1696 2024-11-06T04:47:34Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.50 psi 2024-11-06T04:47:34Z- handleDepth()- Depth_Keller msg: 1730868454.17 epoch sec; seqNo:1824; depth 50.51 2024-11-06T04:47:34Z- handleDVL() - DVL msg epochMillisec: 1730868454.19 2024-11-06T04:47:34Z- handleDvl() - RDI_Pathfinder msg: 1730868454.19 sec; seqNo:665; #beams:4 2024-11-06T04:47:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.25 , 32.96 , 35.51 , 35.51 2024-11-06T04:47:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.09 , -0.01 2024-11-06T04:47:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868454.20 epoch sec; seqNo:1824; 4061695.27 north; 594540.36 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T04:47:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868450.92 2024-11-06T04:47:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:34Z- waiting for 1730868454.92, time is 1730868454.20 2024-11-06T04:47:34Z- AHRS_M2 msg: 1730868454.55 epoch sec; seqNo:1697 2024-11-06T04:47:34Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.50 psi 2024-11-06T04:47:34Z- handleDepth()- Depth_Keller msg: 1730868454.56 epoch sec; seqNo:1825; depth 50.54 2024-11-06T04:47:34Z- handleDVL() - DVL msg epochMillisec: 1730868454.56 2024-11-06T04:47:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:34Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:34Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:34Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:34Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:34Z- handleDvl() - RDI_Pathfinder msg: 1730868454.56 sec; seqNo:666; #beams:0 2024-11-06T04:47:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.09 , -0.01 2024-11-06T04:47:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868454.58 epoch sec; seqNo:1825; 4061695.27 north; 594540.36 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T04:47:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868450.92 2024-11-06T04:47:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:34Z- waiting for 1730868454.92, time is 1730868454.58 2024-11-06T04:47:34Z- AHRS_M2 msg: 1730868454.96 epoch sec; seqNo:1698 2024-11-06T04:47:34Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 1.50 psi 2024-11-06T04:47:34Z- handleDepth()- Depth_Keller msg: 1730868454.96 epoch sec; seqNo:1826; depth 50.52 2024-11-06T04:47:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868454.97 epoch sec; seqNo:1826; 4061695.28 north; 594541.58 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T04:47:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868450.92 2024-11-06T04:47:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:35Z- waiting for 1730868454.92, time is 1730868454.97 2024-11-06T04:47:35Z- LcmTrn::updateTrn() >>>> pose time 1730868454.97, meas time 1730868454.56 <<<< 2024-11-06T04:47:35Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868454.97 2024-11-06T04:47:35Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:47:35Z- TerrainNav::motionUpdate() - currEstimate = (4061695, 594541.58, 50.52) 2024-11-06T04:47:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:47:35Z- TerrainNav:: SubcloudNIS is 0.43, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:47:35Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:47:35Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:47:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:47:35Z- variances([0], [2], and psi_berg) = 0.372474 0.521347 0.000000 2024-11-06T04:47:35Z- publishEstimatesDecoder() - at 1730868454.97 sec 2024-11-06T04:47:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:47:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061639.86 594545.74 55.50 2024-11-06T04:47:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061639.06 594545.98 55.75 lat/lon: 36.70 -121.94 2024-11-06T04:47:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.37 0.52 1.16 0.00 2024-11-06T04:47:35Z- AHRS_M2 msg: 1730868455.36 epoch sec; seqNo:1699 2024-11-06T04:47:35Z- AHRS_M2 msg: 0.12 phi; 0.00 theta; 1.50 psi 2024-11-06T04:47:35Z- handleDepth()- Depth_Keller msg: 1730868455.38 epoch sec; seqNo:1827; depth 50.53 2024-11-06T04:47:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868455.39 epoch sec; seqNo:1827; 4061695.28 north; 594541.58 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T04:47:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868454.97 2024-11-06T04:47:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:35Z- waiting for 1730868458.97, time is 1730868455.39 2024-11-06T04:47:35Z- AHRS_M2 msg: 1730868455.77 epoch sec; seqNo:1700 2024-11-06T04:47:35Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.50 psi 2024-11-06T04:47:35Z- handleDepth()- Depth_Keller msg: 1730868455.77 epoch sec; seqNo:1828; depth 50.55 2024-11-06T04:47:35Z- handleDVL() - DVL msg epochMillisec: 1730868455.77 2024-11-06T04:47:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:35Z- handleDvl() - RDI_Pathfinder msg: 1730868455.77 sec; seqNo:667; #beams:0 2024-11-06T04:47:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.09 , -0.01 2024-11-06T04:47:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868455.79 epoch sec; seqNo:1828; 4061695.28 north; 594541.58 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:47:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868454.97 2024-11-06T04:47:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:35Z- waiting for 1730868458.97, time is 1730868455.79 2024-11-06T04:47:36Z- AHRS_M2 msg: 1730868456.17 epoch sec; seqNo:1701 2024-11-06T04:47:36Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.50 psi 2024-11-06T04:47:36Z- handleDepth()- Depth_Keller msg: 1730868456.17 epoch sec; seqNo:1829; depth 50.54 2024-11-06T04:47:36Z- handleDVL() - DVL msg epochMillisec: 1730868456.19 2024-11-06T04:47:36Z- handleDvl() - RDI_Pathfinder msg: 1730868456.19 sec; seqNo:668; #beams:4 2024-11-06T04:47:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.42 , 33.30 , 35.85 , 35.51 2024-11-06T04:47:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.05 , 0.07 2024-11-06T04:47:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868456.22 epoch sec; seqNo:1829; 4061695.28 north; 594541.58 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T04:47:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868454.97 2024-11-06T04:47:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:36Z- waiting for 1730868458.97, time is 1730868456.22 2024-11-06T04:47:36Z- AHRS_M2 msg: 1730868456.57 epoch sec; seqNo:1702 2024-11-06T04:47:36Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.50 psi 2024-11-06T04:47:36Z- handleDepth()- Depth_Keller msg: 1730868456.58 epoch sec; seqNo:1830; depth 50.55 2024-11-06T04:47:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868456.59 epoch sec; seqNo:1830; 4061695.67 north; 594542.79 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:47:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868454.97 2024-11-06T04:47:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:36Z- waiting for 1730868458.97, time is 1730868456.59 2024-11-06T04:47:36Z- AHRS_M2 msg: 1730868456.97 epoch sec; seqNo:1703 2024-11-06T04:47:36Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.50 psi 2024-11-06T04:47:36Z- handleDepth()- Depth_Keller msg: 1730868456.98 epoch sec; seqNo:1831; depth 50.57 2024-11-06T04:47:36Z- handleDVL() - DVL msg epochMillisec: 1730868456.98 2024-11-06T04:47:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:36Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:37Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:37Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:37Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:37Z- handleDvl() - RDI_Pathfinder msg: 1730868456.98 sec; seqNo:669; #beams:0 2024-11-06T04:47:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.05 , 0.07 2024-11-06T04:47:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868457.00 epoch sec; seqNo:1831; 4061695.67 north; 594542.79 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T04:47:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868454.97 2024-11-06T04:47:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:37Z- waiting for 1730868458.97, time is 1730868457.00 2024-11-06T04:47:37Z- AHRS_M2 msg: 1730868457.39 epoch sec; seqNo:1704 2024-11-06T04:47:37Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.50 psi 2024-11-06T04:47:37Z- handleDepth()- Depth_Keller msg: 1730868457.39 epoch sec; seqNo:1832; depth 50.58 2024-11-06T04:47:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868457.40 epoch sec; seqNo:1832; 4061695.67 north; 594542.79 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T04:47:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868454.97 2024-11-06T04:47:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:37Z- waiting for 1730868458.97, time is 1730868457.40 2024-11-06T04:47:37Z- AHRS_M2 msg: 1730868457.78 epoch sec; seqNo:1705 2024-11-06T04:47:37Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.50 psi 2024-11-06T04:47:37Z- handleDepth()- Depth_Keller msg: 1730868457.79 epoch sec; seqNo:1833; depth 50.58 2024-11-06T04:47:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868457.80 epoch sec; seqNo:1833; 4061695.67 north; 594542.79 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T04:47:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868454.97 2024-11-06T04:47:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:37Z- waiting for 1730868458.97, time is 1730868457.80 2024-11-06T04:47:38Z- AHRS_M2 msg: 1730868458.19 epoch sec; seqNo:1706 2024-11-06T04:47:38Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.50 psi 2024-11-06T04:47:38Z- handleDepth()- Depth_Keller msg: 1730868458.21 epoch sec; seqNo:1834; depth 50.58 2024-11-06T04:47:38Z- handleDVL() - DVL msg epochMillisec: 1730868458.23 2024-11-06T04:47:38Z- handleDvl() - RDI_Pathfinder msg: 1730868458.23 sec; seqNo:670; #beams:4 2024-11-06T04:47:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.42 , 33.47 , 36.19 , 35.68 2024-11-06T04:47:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.00 , 0.08 2024-11-06T04:47:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868458.24 epoch sec; seqNo:1834; 4061695.69 north; 594544.01 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T04:47:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868454.97 2024-11-06T04:47:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:38Z- waiting for 1730868458.97, time is 1730868458.24 2024-11-06T04:47:38Z- AHRS_M2 msg: 1730868458.59 epoch sec; seqNo:1707 2024-11-06T04:47:38Z- AHRS_M2 msg: 0.12 phi; 0.01 theta; 1.50 psi 2024-11-06T04:47:38Z- handleDepth()- Depth_Keller msg: 1730868458.60 epoch sec; seqNo:1835; depth 50.59 2024-11-06T04:47:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868458.62 epoch sec; seqNo:1835; 4061695.69 north; 594544.01 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T04:47:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868454.97 2024-11-06T04:47:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:38Z- waiting for 1730868458.97, time is 1730868458.62 2024-11-06T04:47:39Z- AHRS_M2 msg: 1730868458.99 epoch sec; seqNo:1708 2024-11-06T04:47:39Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.50 psi 2024-11-06T04:47:39Z- handleDepth()- Depth_Keller msg: 1730868459.01 epoch sec; seqNo:1836; depth 50.58 2024-11-06T04:47:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868459.02 epoch sec; seqNo:1836; 4061695.69 north; 594544.01 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T04:47:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868454.97 2024-11-06T04:47:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:39Z- waiting for 1730868458.97, time is 1730868459.02 2024-11-06T04:47:39Z- LcmTrn::updateTrn() >>>> pose time 1730868459.02, meas time 1730868458.23 <<<< 2024-11-06T04:47:39Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868459.02 2024-11-06T04:47:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868458.23 sec, ping # 0; waiting for INS data more recent than 1730868454.97... 2024-11-06T04:47:39Z- TerrainNav::motionUpdate() - currEstimate = (4061696, 594544.01, 50.58) 2024-11-06T04:47:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:47:39Z- TerrainNav:: SubcloudNIS is 0.43, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:47:39Z- TNavPF::Projecting Measurements 2024-11-06T04:47:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:47:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:47:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:47:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:47:39Z- TNavFilter::4 good beams. 2024-11-06T04:47:39Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:47:39Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:47:39Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:47:39Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:47:39Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:47:39Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:47:39Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:47:39Z- TerrainMapOctree: (vehN, vehE ) = (4061695.61, 594543.54). 2024-11-06T04:47:39Z- TerrainMapOctree: Min Distance = 213.50. 2024-11-06T04:47:39Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:47:39Z- Forcing Subcloud Comparison 2024-11-06T04:47:39Z- Weighting particles with subcloud comparison 2024-11-06T04:47:39Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:39Z- meanExpectedMeasDiff: 0.704055 delta_rms_squared: 2.219315 alpha: 0.991069 2024-11-06T04:47:39Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:39Z- meanExpectedMeasDiff: -0.064318 delta_rms_squared: 1.662791 alpha: 0.988115 2024-11-06T04:47:39Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:39Z- meanExpectedMeasDiff: 1.163597 delta_rms_squared: 1.938853 alpha: 0.989790 2024-11-06T04:47:39Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:39Z- meanExpectedMeasDiff: -0.003334 delta_rms_squared: 1.883946 alpha: 0.989495 2024-11-06T04:47:39Z- Subcloud NIS: 0.400992 2024-11-06T04:47:39Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:47:39Z- TNavPF:: sumWeights = 0.492198 2024-11-06T04:47:39Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:47:39Z- TNavPF::First Term of Map Covariance Matrix is 0.38 2024-11-06T04:47:39Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T04:47:39Z- TerrainNav::Resampling particle filter... 2024-11-06T04:47:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868458.23 sec. 2024-11-06T04:47:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:47:39Z- variances([0], [2], and psi_berg) = 0.376630 0.524602 0.000000 2024-11-06T04:47:39Z- publishEstimatesDecoder() - at 1730868459.02 sec 2024-11-06T04:47:39Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:47:39Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061639.24 594547.56 54.97 2024-11-06T04:47:39Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061639.47 594548.42 55.55 lat/lon: 36.70 -121.94 2024-11-06T04:47:39Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.38 0.52 0.73 0.00 2024-11-06T04:47:39Z- AHRS_M2 msg: 1730868459.40 epoch sec; seqNo:1709 2024-11-06T04:47:39Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.50 psi 2024-11-06T04:47:39Z- handleDepth()- Depth_Keller msg: 1730868459.42 epoch sec; seqNo:1837; depth 50.60 2024-11-06T04:47:39Z- handleDVL() - DVL msg epochMillisec: 1730868459.42 2024-11-06T04:47:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:39Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:39Z- handleDvl() - RDI_Pathfinder msg: 1730868459.42 sec; seqNo:671; #beams:0 2024-11-06T04:47:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.00 , 0.08 2024-11-06T04:47:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868459.44 epoch sec; seqNo:1837; 4061695.70 north; 594545.23 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T04:47:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868459.02 2024-11-06T04:47:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:39Z- waiting for 1730868463.02, time is 1730868459.44 2024-11-06T04:47:39Z- AHRS_M2 msg: 1730868459.81 epoch sec; seqNo:1710 2024-11-06T04:47:39Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.50 psi 2024-11-06T04:47:39Z- handleDepth()- Depth_Keller msg: 1730868459.82 epoch sec; seqNo:1838; depth 50.59 2024-11-06T04:47:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868459.84 epoch sec; seqNo:1838; 4061695.70 north; 594545.23 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T04:47:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868459.02 2024-11-06T04:47:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:39Z- waiting for 1730868463.02, time is 1730868459.84 2024-11-06T04:47:40Z- AHRS_M2 msg: 1730868460.21 epoch sec; seqNo:1711 2024-11-06T04:47:40Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.50 psi 2024-11-06T04:47:40Z- handleDepth()- Depth_Keller msg: 1730868460.22 epoch sec; seqNo:1839; depth 50.58 2024-11-06T04:47:40Z- handleDVL() - DVL msg epochMillisec: 1730868460.24 2024-11-06T04:47:40Z- handleDvl() - RDI_Pathfinder msg: 1730868460.24 sec; seqNo:672; #beams:4 2024-11-06T04:47:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.42 , 33.47 , 36.36 , 35.51 2024-11-06T04:47:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , 0.04 2024-11-06T04:47:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868460.26 epoch sec; seqNo:1839; 4061695.70 north; 594545.23 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T04:47:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868459.02 2024-11-06T04:47:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:40Z- waiting for 1730868463.02, time is 1730868460.26 2024-11-06T04:47:40Z- AHRS_M2 msg: 1730868460.62 epoch sec; seqNo:1712 2024-11-06T04:47:40Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.50 psi 2024-11-06T04:47:40Z- handleDepth()- Depth_Keller msg: 1730868460.62 epoch sec; seqNo:1840; depth 50.58 2024-11-06T04:47:40Z- handleDVL() - DVL msg epochMillisec: 1730868460.62 2024-11-06T04:47:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:40Z- handleDvl() - RDI_Pathfinder msg: 1730868460.62 sec; seqNo:673; #beams:0 2024-11-06T04:47:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , 0.04 2024-11-06T04:47:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868460.64 epoch sec; seqNo:1840; 4061695.70 north; 594545.23 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T04:47:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868459.02 2024-11-06T04:47:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:40Z- waiting for 1730868463.02, time is 1730868460.64 2024-11-06T04:47:41Z- AHRS_M2 msg: 1730868461.02 epoch sec; seqNo:1713 2024-11-06T04:47:41Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.50 psi 2024-11-06T04:47:41Z- handleDepth()- Depth_Keller msg: 1730868461.02 epoch sec; seqNo:1841; depth 50.58 2024-11-06T04:47:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868461.04 epoch sec; seqNo:1841; 4061696.09 north; 594546.45 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T04:47:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868459.02 2024-11-06T04:47:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:41Z- waiting for 1730868463.02, time is 1730868461.04 2024-11-06T04:47:41Z- AHRS_M2 msg: 1730868461.42 epoch sec; seqNo:1714 2024-11-06T04:47:41Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.50 psi 2024-11-06T04:47:41Z- handleDepth()- Depth_Keller msg: 1730868461.43 epoch sec; seqNo:1842; depth 50.57 2024-11-06T04:47:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868461.44 epoch sec; seqNo:1842; 4061696.09 north; 594546.45 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T04:47:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868459.02 2024-11-06T04:47:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:41Z- waiting for 1730868463.02, time is 1730868461.44 2024-11-06T04:47:41Z- AHRS_M2 msg: 1730868461.83 epoch sec; seqNo:1715 2024-11-06T04:47:41Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.51 psi 2024-11-06T04:47:41Z- handleDepth()- Depth_Keller msg: 1730868461.84 epoch sec; seqNo:1843; depth 50.57 2024-11-06T04:47:41Z- handleDVL() - DVL msg epochMillisec: 1730868461.84 2024-11-06T04:47:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:41Z- handleDvl() - RDI_Pathfinder msg: 1730868461.84 sec; seqNo:674; #beams:0 2024-11-06T04:47:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , 0.04 2024-11-06T04:47:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868461.85 epoch sec; seqNo:1843; 4061696.09 north; 594546.45 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T04:47:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868459.02 2024-11-06T04:47:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:41Z- waiting for 1730868463.02, time is 1730868461.85 2024-11-06T04:47:42Z- AHRS_M2 msg: 1730868462.23 epoch sec; seqNo:1716 2024-11-06T04:47:42Z- AHRS_M2 msg: 0.12 phi; 0.02 theta; 1.51 psi 2024-11-06T04:47:42Z- handleDepth()- Depth_Keller msg: 1730868462.24 epoch sec; seqNo:1844; depth 50.54 2024-11-06T04:47:42Z- handleDVL() - DVL msg epochMillisec: 1730868462.26 2024-11-06T04:47:42Z- handleDvl() - RDI_Pathfinder msg: 1730868462.26 sec; seqNo:675; #beams:4 2024-11-06T04:47:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.76 , 33.64 , 36.53 , 35.51 2024-11-06T04:47:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.03 2024-11-06T04:47:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868462.28 epoch sec; seqNo:1844; 4061696.11 north; 594547.67 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T04:47:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868459.02 2024-11-06T04:47:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:42Z- waiting for 1730868463.02, time is 1730868462.28 2024-11-06T04:47:42Z- AHRS_M2 msg: 1730868462.64 epoch sec; seqNo:1717 2024-11-06T04:47:42Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.51 psi 2024-11-06T04:47:42Z- handleDepth()- Depth_Keller msg: 1730868462.65 epoch sec; seqNo:1845; depth 50.53 2024-11-06T04:47:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868462.67 epoch sec; seqNo:1845; 4061696.11 north; 594547.67 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T04:47:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868459.02 2024-11-06T04:47:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:42Z- waiting for 1730868463.02, time is 1730868462.67 2024-11-06T04:47:43Z- AHRS_M2 msg: 1730868463.04 epoch sec; seqNo:1718 2024-11-06T04:47:43Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.51 psi 2024-11-06T04:47:43Z- handleDepth()- Depth_Keller msg: 1730868463.05 epoch sec; seqNo:1846; depth 50.50 2024-11-06T04:47:43Z- handleDVL() - DVL msg epochMillisec: 1730868463.05 2024-11-06T04:47:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:43Z- handleDvl() - RDI_Pathfinder msg: 1730868463.05 sec; seqNo:676; #beams:0 2024-11-06T04:47:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.03 2024-11-06T04:47:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868463.06 epoch sec; seqNo:1846; 4061696.11 north; 594547.67 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T04:47:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868459.02 2024-11-06T04:47:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:43Z- waiting for 1730868463.02, time is 1730868463.06 2024-11-06T04:47:43Z- LcmTrn::updateTrn() >>>> pose time 1730868463.06, meas time 1730868463.05 <<<< 2024-11-06T04:47:43Z- LcmTrn::updateTrn() equating pose 1730868463.05 and meas times 1730868463.05 2024-11-06T04:47:43Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868463.05 2024-11-06T04:47:43Z- TerrainNav::motionUpdate() - currEstimate = (4061696, 594547.67, 50.50) 2024-11-06T04:47:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:47:43Z- TerrainNav:: SubcloudNIS is 0.40, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:47:43Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:47:43Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:47:43Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:47:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:47:43Z- variances([0], [2], and psi_berg) = 0.378204 0.527536 0.000000 2024-11-06T04:47:43Z- publishEstimatesDecoder() - at 1730868463.05 sec 2024-11-06T04:47:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:47:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061639.71 594551.20 54.88 2024-11-06T04:47:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061639.89 594552.08 55.47 lat/lon: 36.70 -121.94 2024-11-06T04:47:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.38 0.53 0.99 0.00 2024-11-06T04:47:43Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:47:43Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:47:43Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:47:43Z- AHRS_M2 msg: 1730868463.44 epoch sec; seqNo:1719 2024-11-06T04:47:43Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.51 psi 2024-11-06T04:47:43Z- handleDepth()- Depth_Keller msg: 1730868463.47 epoch sec; seqNo:1847; depth 50.50 2024-11-06T04:47:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868463.48 epoch sec; seqNo:1847; 4061696.12 north; 594548.89 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T04:47:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868463.05 2024-11-06T04:47:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:43Z- waiting for 1730868467.05, time is 1730868463.48 2024-11-06T04:47:43Z- AHRS_M2 msg: 1730868463.85 epoch sec; seqNo:1720 2024-11-06T04:47:43Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.51 psi 2024-11-06T04:47:43Z- handleDepth()- Depth_Keller msg: 1730868463.86 epoch sec; seqNo:1848; depth 50.46 2024-11-06T04:47:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868463.87 epoch sec; seqNo:1848; 4061696.12 north; 594548.89 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T04:47:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868463.05 2024-11-06T04:47:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:44Z- waiting for 1730868467.05, time is 1730868463.87 2024-11-06T04:47:44Z- AHRS_M2 msg: 1730868464.26 epoch sec; seqNo:1721 2024-11-06T04:47:44Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.51 psi 2024-11-06T04:47:44Z- handleDepth()- Depth_Keller msg: 1730868464.27 epoch sec; seqNo:1849; depth 50.45 2024-11-06T04:47:44Z- handleDVL() - DVL msg epochMillisec: 1730868464.29 2024-11-06T04:47:44Z- handleDvl() - RDI_Pathfinder msg: 1730868464.29 sec; seqNo:677; #beams:4 2024-11-06T04:47:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.59 , 33.64 , 36.53 , 35.34 2024-11-06T04:47:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.06 2024-11-06T04:47:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868464.31 epoch sec; seqNo:1849; 4061696.12 north; 594548.89 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T04:47:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868463.05 2024-11-06T04:47:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:44Z- waiting for 1730868467.05, time is 1730868464.31 2024-11-06T04:47:44Z- AHRS_M2 msg: 1730868464.66 epoch sec; seqNo:1722 2024-11-06T04:47:44Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.52 psi 2024-11-06T04:47:44Z- handleDepth()- Depth_Keller msg: 1730868464.66 epoch sec; seqNo:1850; depth 50.42 2024-11-06T04:47:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868464.68 epoch sec; seqNo:1850; 4061696.12 north; 594548.89 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T04:47:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868463.05 2024-11-06T04:47:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:44Z- waiting for 1730868467.05, time is 1730868464.68 2024-11-06T04:47:45Z- AHRS_M2 msg: 1730868465.06 epoch sec; seqNo:1723 2024-11-06T04:47:45Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.52 psi 2024-11-06T04:47:45Z- handleDepth()- Depth_Keller msg: 1730868465.07 epoch sec; seqNo:1851; depth 50.41 2024-11-06T04:47:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868465.08 epoch sec; seqNo:1851; 4061696.13 north; 594550.11 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T04:47:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868463.05 2024-11-06T04:47:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:45Z- waiting for 1730868467.05, time is 1730868465.08 2024-11-06T04:47:45Z- AHRS_M2 msg: 1730868465.46 epoch sec; seqNo:1724 2024-11-06T04:47:45Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.52 psi 2024-11-06T04:47:45Z- handleDepth()- Depth_Keller msg: 1730868465.47 epoch sec; seqNo:1852; depth 50.37 2024-11-06T04:47:45Z- handleDVL() - DVL msg epochMillisec: 1730868465.47 2024-11-06T04:47:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:45Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:45Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:45Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:45Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:45Z- handleDvl() - RDI_Pathfinder msg: 1730868465.47 sec; seqNo:678; #beams:0 2024-11-06T04:47:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.06 2024-11-06T04:47:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868465.48 epoch sec; seqNo:1852; 4061696.13 north; 594550.11 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:47:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868463.05 2024-11-06T04:47:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:45Z- waiting for 1730868467.05, time is 1730868465.48 2024-11-06T04:47:45Z- AHRS_M2 msg: 1730868465.87 epoch sec; seqNo:1725 2024-11-06T04:47:45Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.52 psi 2024-11-06T04:47:45Z- handleDepth()- Depth_Keller msg: 1730868465.88 epoch sec; seqNo:1853; depth 50.36 2024-11-06T04:47:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868465.90 epoch sec; seqNo:1853; 4061696.13 north; 594550.11 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:47:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868463.05 2024-11-06T04:47:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:46Z- waiting for 1730868467.05, time is 1730868465.90 2024-11-06T04:47:46Z- AHRS_M2 msg: 1730868466.28 epoch sec; seqNo:1726 2024-11-06T04:47:46Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.52 psi 2024-11-06T04:47:46Z- handleDepth()- Depth_Keller msg: 1730868466.28 epoch sec; seqNo:1854; depth 50.31 2024-11-06T04:47:46Z- handleDVL() - DVL msg epochMillisec: 1730868466.30 2024-11-06T04:47:46Z- handleDvl() - RDI_Pathfinder msg: 1730868466.30 sec; seqNo:679; #beams:4 2024-11-06T04:47:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.59 , 33.64 , 36.87 , 35.34 2024-11-06T04:47:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.09 , -0.05 2024-11-06T04:47:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868466.32 epoch sec; seqNo:1854; 4061696.15 north; 594551.33 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:47:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868463.05 2024-11-06T04:47:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:46Z- waiting for 1730868467.05, time is 1730868466.32 2024-11-06T04:47:46Z- AHRS_M2 msg: 1730868466.68 epoch sec; seqNo:1727 2024-11-06T04:47:46Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.52 psi 2024-11-06T04:47:46Z- handleDepth()- Depth_Keller msg: 1730868466.69 epoch sec; seqNo:1855; depth 50.30 2024-11-06T04:47:46Z- handleDVL() - DVL msg epochMillisec: 1730868466.69 2024-11-06T04:47:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:46Z- handleDvl() - RDI_Pathfinder msg: 1730868466.69 sec; seqNo:680; #beams:0 2024-11-06T04:47:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.09 , -0.05 2024-11-06T04:47:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868466.71 epoch sec; seqNo:1855; 4061696.15 north; 594551.33 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:47:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868463.05 2024-11-06T04:47:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:46Z- waiting for 1730868467.05, time is 1730868466.71 2024-11-06T04:47:47Z- AHRS_M2 msg: 1730868467.08 epoch sec; seqNo:1728 2024-11-06T04:47:47Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.52 psi 2024-11-06T04:47:47Z- handleDepth()- Depth_Keller msg: 1730868467.09 epoch sec; seqNo:1856; depth 50.29 2024-11-06T04:47:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868467.10 epoch sec; seqNo:1856; 4061696.53 north; 594551.33 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:47:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868463.05 2024-11-06T04:47:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:47Z- waiting for 1730868467.05, time is 1730868467.10 2024-11-06T04:47:47Z- LcmTrn::updateTrn() >>>> pose time 1730868467.10, meas time 1730868466.69 <<<< 2024-11-06T04:47:47Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868467.10 2024-11-06T04:47:47Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:47:47Z- TerrainNav::motionUpdate() - currEstimate = (4061697, 594551.33, 50.29) 2024-11-06T04:47:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:47:47Z- TerrainNav:: SubcloudNIS is 0.40, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:47:47Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:47:47Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:47:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:47:47Z- variances([0], [2], and psi_berg) = 0.380703 0.529859 0.000000 2024-11-06T04:47:47Z- publishEstimatesDecoder() - at 1730868467.10 sec 2024-11-06T04:47:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:47:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061640.14 594554.95 54.63 2024-11-06T04:47:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061640.31 594555.73 55.27 lat/lon: 36.70 -121.94 2024-11-06T04:47:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.38 0.53 1.25 0.00 2024-11-06T04:47:47Z- AHRS_M2 msg: 1730868467.48 epoch sec; seqNo:1729 2024-11-06T04:47:47Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.51 psi 2024-11-06T04:47:47Z- handleDepth()- Depth_Keller msg: 1730868467.49 epoch sec; seqNo:1857; depth 50.26 2024-11-06T04:47:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868467.50 epoch sec; seqNo:1857; 4061696.53 north; 594551.33 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:47:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868467.10 2024-11-06T04:47:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:47Z- waiting for 1730868471.10, time is 1730868467.50 2024-11-06T04:47:47Z- AHRS_M2 msg: 1730868467.89 epoch sec; seqNo:1730 2024-11-06T04:47:47Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.51 psi 2024-11-06T04:47:47Z- handleDepth()- Depth_Keller msg: 1730868467.93 epoch sec; seqNo:1858; depth 50.25 2024-11-06T04:47:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868467.96 epoch sec; seqNo:1858; 4061696.54 north; 594552.55 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T04:47:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868467.10 2024-11-06T04:47:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:48Z- waiting for 1730868471.10, time is 1730868467.96 2024-11-06T04:47:48Z- AHRS_M2 msg: 1730868468.30 epoch sec; seqNo:1731 2024-11-06T04:47:48Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.51 psi 2024-11-06T04:47:48Z- handleDepth()- Depth_Keller msg: 1730868468.30 epoch sec; seqNo:1859; depth 50.22 2024-11-06T04:47:48Z- handleDVL() - DVL msg epochMillisec: 1730868468.33 2024-11-06T04:47:48Z- handleDvl() - RDI_Pathfinder msg: 1730868468.33 sec; seqNo:681; #beams:4 2024-11-06T04:47:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.59 , 33.64 , 36.70 , 35.17 2024-11-06T04:47:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.08 , -0.01 2024-11-06T04:47:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868468.34 epoch sec; seqNo:1859; 4061696.54 north; 594552.55 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T04:47:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868467.10 2024-11-06T04:47:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:48Z- waiting for 1730868471.10, time is 1730868468.34 2024-11-06T04:47:48Z- AHRS_M2 msg: 1730868468.70 epoch sec; seqNo:1732 2024-11-06T04:47:48Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.51 psi 2024-11-06T04:47:48Z- handleDepth()- Depth_Keller msg: 1730868468.70 epoch sec; seqNo:1860; depth 50.19 2024-11-06T04:47:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868468.72 epoch sec; seqNo:1860; 4061696.54 north; 594552.55 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:47:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868467.10 2024-11-06T04:47:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:48Z- waiting for 1730868471.10, time is 1730868468.72 2024-11-06T04:47:49Z- AHRS_M2 msg: 1730868469.10 epoch sec; seqNo:1733 2024-11-06T04:47:49Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 1.51 psi 2024-11-06T04:47:49Z- handleDepth()- Depth_Keller msg: 1730868469.11 epoch sec; seqNo:1861; depth 50.20 2024-11-06T04:47:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868469.11 epoch sec; seqNo:1861; 4061696.54 north; 594552.55 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:47:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868467.10 2024-11-06T04:47:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:49Z- waiting for 1730868471.10, time is 1730868469.11 2024-11-06T04:47:49Z- AHRS_M2 msg: 1730868469.50 epoch sec; seqNo:1734 2024-11-06T04:47:49Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 1.51 psi 2024-11-06T04:47:49Z- handleDepth()- Depth_Keller msg: 1730868469.51 epoch sec; seqNo:1862; depth 50.18 2024-11-06T04:47:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868469.52 epoch sec; seqNo:1862; 4061696.55 north; 594553.77 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T04:47:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868467.10 2024-11-06T04:47:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:49Z- waiting for 1730868471.10, time is 1730868469.52 2024-11-06T04:47:49Z- AHRS_M2 msg: 1730868469.91 epoch sec; seqNo:1735 2024-11-06T04:47:49Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 1.50 psi 2024-11-06T04:47:49Z- handleDepth()- Depth_Keller msg: 1730868469.91 epoch sec; seqNo:1863; depth 50.16 2024-11-06T04:47:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868469.92 epoch sec; seqNo:1863; 4061696.55 north; 594553.77 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T04:47:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868467.10 2024-11-06T04:47:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:50Z- waiting for 1730868471.10, time is 1730868469.92 2024-11-06T04:47:50Z- AHRS_M2 msg: 1730868470.31 epoch sec; seqNo:1736 2024-11-06T04:47:50Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 1.50 psi 2024-11-06T04:47:50Z- handleDepth()- Depth_Keller msg: 1730868470.32 epoch sec; seqNo:1864; depth 50.14 2024-11-06T04:47:50Z- handleDVL() - DVL msg epochMillisec: 1730868470.34 2024-11-06T04:47:50Z- handleDvl() - RDI_Pathfinder msg: 1730868470.34 sec; seqNo:682; #beams:4 2024-11-06T04:47:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.93 , 33.64 , 36.87 , 35.51 2024-11-06T04:47:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.07 , 0.02 2024-11-06T04:47:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868470.36 epoch sec; seqNo:1864; 4061696.93 north; 594553.76 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:47:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868467.10 2024-11-06T04:47:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:50Z- waiting for 1730868471.10, time is 1730868470.36 2024-11-06T04:47:50Z- AHRS_M2 msg: 1730868470.72 epoch sec; seqNo:1737 2024-11-06T04:47:50Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 1.50 psi 2024-11-06T04:47:50Z- handleDepth()- Depth_Keller msg: 1730868470.73 epoch sec; seqNo:1865; depth 50.15 2024-11-06T04:47:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868470.74 epoch sec; seqNo:1865; 4061696.93 north; 594553.76 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T04:47:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868467.10 2024-11-06T04:47:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:50Z- waiting for 1730868471.10, time is 1730868470.74 2024-11-06T04:47:51Z- AHRS_M2 msg: 1730868471.12 epoch sec; seqNo:1738 2024-11-06T04:47:51Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 1.50 psi 2024-11-06T04:47:51Z- handleDepth()- Depth_Keller msg: 1730868471.13 epoch sec; seqNo:1866; depth 50.13 2024-11-06T04:47:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868471.14 epoch sec; seqNo:1866; 4061696.94 north; 594554.98 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T04:47:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868467.10 2024-11-06T04:47:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:51Z- waiting for 1730868471.10, time is 1730868471.14 2024-11-06T04:47:51Z- LcmTrn::updateTrn() >>>> pose time 1730868471.14, meas time 1730868470.34 <<<< 2024-11-06T04:47:51Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868471.14 2024-11-06T04:47:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868470.34 sec, ping # 0; waiting for INS data more recent than 1730868467.10... 2024-11-06T04:47:51Z- TerrainNav::motionUpdate() - currEstimate = (4061697, 594554.98, 50.13) 2024-11-06T04:47:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:47:51Z- TerrainNav:: SubcloudNIS is 0.40, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:47:51Z- TNavPF::Projecting Measurements 2024-11-06T04:47:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:47:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:47:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:47:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:47:51Z- TNavFilter::4 good beams. 2024-11-06T04:47:51Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:47:51Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:47:51Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:47:51Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:47:51Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:47:51Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:47:51Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:47:51Z- TerrainMapOctree: (vehN, vehE ) = (4061696.86, 594554.26). 2024-11-06T04:47:51Z- TerrainMapOctree: Min Distance = 202.83. 2024-11-06T04:47:51Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:47:51Z- Forcing Subcloud Comparison 2024-11-06T04:47:51Z- Weighting particles with subcloud comparison 2024-11-06T04:47:51Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:51Z- meanExpectedMeasDiff: 0.422941 delta_rms_squared: 2.374833 alpha: 0.991649 2024-11-06T04:47:51Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:51Z- meanExpectedMeasDiff: 0.434448 delta_rms_squared: 1.787140 alpha: 0.988933 2024-11-06T04:47:51Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:51Z- meanExpectedMeasDiff: 2.286419 delta_rms_squared: 2.282840 alpha: 0.991315 2024-11-06T04:47:51Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:47:51Z- meanExpectedMeasDiff: -0.510468 delta_rms_squared: 1.972673 alpha: 0.989963 2024-11-06T04:47:51Z- Subcloud NIS: 0.386681 2024-11-06T04:47:51Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:47:51Z- TNavPF:: sumWeights = 0.304458 2024-11-06T04:47:51Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:47:51Z- TNavPF::First Term of Map Covariance Matrix is 0.24 2024-11-06T04:47:51Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T04:47:51Z- TerrainNav::Resampling particle filter... 2024-11-06T04:47:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868470.34 sec. 2024-11-06T04:47:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:47:51Z- variances([0], [2], and psi_berg) = 0.377318 0.540258 0.000000 2024-11-06T04:47:51Z- publishEstimatesDecoder() - at 1730868471.14 sec 2024-11-06T04:47:51Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:47:51Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061640.55 594558.53 53.59 2024-11-06T04:47:51Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061640.63 594559.47 54.72 lat/lon: 36.70 -121.94 2024-11-06T04:47:51Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.38 0.54 0.76 0.00 2024-11-06T04:47:51Z- AHRS_M2 msg: 1730868471.53 epoch sec; seqNo:1739 2024-11-06T04:47:51Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 1.50 psi 2024-11-06T04:47:51Z- handleDepth()- Depth_Keller msg: 1730868471.54 epoch sec; seqNo:1867; depth 50.12 2024-11-06T04:47:51Z- handleDVL() - DVL msg epochMillisec: 1730868471.54 2024-11-06T04:47:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:51Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:51Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:51Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:51Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:51Z- handleDvl() - RDI_Pathfinder msg: 1730868471.54 sec; seqNo:683; #beams:0 2024-11-06T04:47:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.07 , 0.02 2024-11-06T04:47:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868471.56 epoch sec; seqNo:1867; 4061696.94 north; 594554.98 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T04:47:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868471.14 2024-11-06T04:47:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:51Z- waiting for 1730868475.14, time is 1730868471.56 2024-11-06T04:47:51Z- AHRS_M2 msg: 1730868471.93 epoch sec; seqNo:1740 2024-11-06T04:47:51Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 1.50 psi 2024-11-06T04:47:51Z- handleDepth()- Depth_Keller msg: 1730868471.94 epoch sec; seqNo:1868; depth 50.10 2024-11-06T04:47:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868471.95 epoch sec; seqNo:1868; 4061696.94 north; 594554.98 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T04:47:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868471.14 2024-11-06T04:47:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:52Z- waiting for 1730868475.14, time is 1730868471.95 2024-11-06T04:47:52Z- AHRS_M2 msg: 1730868472.34 epoch sec; seqNo:1741 2024-11-06T04:47:52Z- AHRS_M2 msg: 0.12 phi; 0.03 theta; 1.50 psi 2024-11-06T04:47:52Z- handleDepth()- Depth_Keller msg: 1730868472.34 epoch sec; seqNo:1869; depth 50.11 2024-11-06T04:47:52Z- handleDVL() - DVL msg epochMillisec: 1730868472.36 2024-11-06T04:47:52Z- handleDvl() - RDI_Pathfinder msg: 1730868472.36 sec; seqNo:684; #beams:4 2024-11-06T04:47:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.76 , 33.64 , 36.70 , 35.34 2024-11-06T04:47:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.02 , 0.04 2024-11-06T04:47:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868472.38 epoch sec; seqNo:1869; 4061696.94 north; 594554.98 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T04:47:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868471.14 2024-11-06T04:47:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:52Z- waiting for 1730868475.14, time is 1730868472.38 2024-11-06T04:47:52Z- AHRS_M2 msg: 1730868472.74 epoch sec; seqNo:1742 2024-11-06T04:47:52Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.50 psi 2024-11-06T04:47:52Z- handleDepth()- Depth_Keller msg: 1730868472.74 epoch sec; seqNo:1870; depth 50.09 2024-11-06T04:47:52Z- handleDVL() - DVL msg epochMillisec: 1730868472.75 2024-11-06T04:47:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:52Z- handleDvl() - RDI_Pathfinder msg: 1730868472.75 sec; seqNo:685; #beams:0 2024-11-06T04:47:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.02 , 0.04 2024-11-06T04:47:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868472.77 epoch sec; seqNo:1870; 4061696.96 north; 594556.21 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T04:47:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868471.14 2024-11-06T04:47:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:52Z- waiting for 1730868475.14, time is 1730868472.77 2024-11-06T04:47:53Z- AHRS_M2 msg: 1730868473.14 epoch sec; seqNo:1743 2024-11-06T04:47:53Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.50 psi 2024-11-06T04:47:53Z- handleDepth()- Depth_Keller msg: 1730868473.15 epoch sec; seqNo:1871; depth 50.08 2024-11-06T04:47:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868473.16 epoch sec; seqNo:1871; 4061696.96 north; 594556.21 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T04:47:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868471.14 2024-11-06T04:47:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:53Z- waiting for 1730868475.14, time is 1730868473.16 2024-11-06T04:47:53Z- AHRS_M2 msg: 1730868473.55 epoch sec; seqNo:1744 2024-11-06T04:47:53Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.50 psi 2024-11-06T04:47:53Z- handleDepth()- Depth_Keller msg: 1730868473.56 epoch sec; seqNo:1872; depth 50.09 2024-11-06T04:47:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868473.56 epoch sec; seqNo:1872; 4061697.34 north; 594556.20 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T04:47:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868471.14 2024-11-06T04:47:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:53Z- waiting for 1730868475.14, time is 1730868473.56 2024-11-06T04:47:53Z- AHRS_M2 msg: 1730868473.96 epoch sec; seqNo:1745 2024-11-06T04:47:53Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.50 psi 2024-11-06T04:47:53Z- handleDepth()- Depth_Keller msg: 1730868473.96 epoch sec; seqNo:1873; depth 50.06 2024-11-06T04:47:53Z- handleDVL() - DVL msg epochMillisec: 1730868473.96 2024-11-06T04:47:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:53Z- handleDvl() - RDI_Pathfinder msg: 1730868473.96 sec; seqNo:686; #beams:0 2024-11-06T04:47:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.02 , 0.04 2024-11-06T04:47:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868473.97 epoch sec; seqNo:1873; 4061697.34 north; 594556.20 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T04:47:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868471.14 2024-11-06T04:47:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:54Z- waiting for 1730868475.14, time is 1730868473.97 2024-11-06T04:47:54Z- AHRS_M2 msg: 1730868474.36 epoch sec; seqNo:1746 2024-11-06T04:47:54Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.50 psi 2024-11-06T04:47:54Z- handleDepth()- Depth_Keller msg: 1730868474.37 epoch sec; seqNo:1874; depth 50.07 2024-11-06T04:47:54Z- handleDVL() - DVL msg epochMillisec: 1730868474.39 2024-11-06T04:47:54Z- handleDvl() - RDI_Pathfinder msg: 1730868474.39 sec; seqNo:687; #beams:4 2024-11-06T04:47:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.10 , 33.81 , 36.87 , 35.68 2024-11-06T04:47:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.05 2024-11-06T04:47:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868474.41 epoch sec; seqNo:1874; 4061697.35 north; 594557.42 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T04:47:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868471.14 2024-11-06T04:47:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:54Z- waiting for 1730868475.14, time is 1730868474.41 2024-11-06T04:47:54Z- AHRS_M2 msg: 1730868474.78 epoch sec; seqNo:1747 2024-11-06T04:47:54Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.50 psi 2024-11-06T04:47:54Z- handleDepth()- Depth_Keller msg: 1730868474.79 epoch sec; seqNo:1875; depth 50.04 2024-11-06T04:47:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868474.81 epoch sec; seqNo:1875; 4061697.35 north; 594557.42 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T04:47:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868471.14 2024-11-06T04:47:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:54Z- waiting for 1730868475.14, time is 1730868474.81 2024-11-06T04:47:55Z- AHRS_M2 msg: 1730868475.16 epoch sec; seqNo:1748 2024-11-06T04:47:55Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.50 psi 2024-11-06T04:47:55Z- handleDepth()- Depth_Keller msg: 1730868475.17 epoch sec; seqNo:1876; depth 50.04 2024-11-06T04:47:55Z- handleDVL() - DVL msg epochMillisec: 1730868475.17 2024-11-06T04:47:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:55Z- handleDvl() - RDI_Pathfinder msg: 1730868475.17 sec; seqNo:688; #beams:0 2024-11-06T04:47:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.05 2024-11-06T04:47:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868475.18 epoch sec; seqNo:1876; 4061697.35 north; 594557.42 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T04:47:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868471.14 2024-11-06T04:47:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:55Z- waiting for 1730868475.14, time is 1730868475.18 2024-11-06T04:47:55Z- LcmTrn::updateTrn() >>>> pose time 1730868475.18, meas time 1730868475.17 <<<< 2024-11-06T04:47:55Z- LcmTrn::updateTrn() equating pose 1730868475.17 and meas times 1730868475.17 2024-11-06T04:47:55Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868475.17 2024-11-06T04:47:55Z- TerrainNav::motionUpdate() - currEstimate = (4061697, 594557.42, 50.04) 2024-11-06T04:47:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:47:55Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:47:55Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:47:55Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:47:55Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:47:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:47:55Z- variances([0], [2], and psi_berg) = 0.378605 0.540783 0.000000 2024-11-06T04:47:55Z- publishEstimatesDecoder() - at 1730868475.17 sec 2024-11-06T04:47:55Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:47:55Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061640.98 594560.93 53.79 2024-11-06T04:47:55Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061641.03 594561.91 54.63 lat/lon: 36.70 -121.94 2024-11-06T04:47:55Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.38 0.54 1.00 0.00 2024-11-06T04:47:55Z- AHRS_M2 msg: 1730868475.57 epoch sec; seqNo:1749 2024-11-06T04:47:55Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.50 psi 2024-11-06T04:47:55Z- handleDepth()- Depth_Keller msg: 1730868475.58 epoch sec; seqNo:1877; depth 50.02 2024-11-06T04:47:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868475.59 epoch sec; seqNo:1877; 4061697.36 north; 594558.64 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:47:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868475.17 2024-11-06T04:47:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:55Z- waiting for 1730868479.17, time is 1730868475.59 2024-11-06T04:47:55Z- AHRS_M2 msg: 1730868475.97 epoch sec; seqNo:1750 2024-11-06T04:47:55Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T04:47:55Z- handleDepth()- Depth_Keller msg: 1730868475.98 epoch sec; seqNo:1878; depth 50.00 2024-11-06T04:47:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868475.99 epoch sec; seqNo:1878; 4061697.36 north; 594558.64 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T04:47:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868475.17 2024-11-06T04:47:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:56Z- waiting for 1730868479.17, time is 1730868475.99 2024-11-06T04:47:56Z- AHRS_M2 msg: 1730868476.38 epoch sec; seqNo:1751 2024-11-06T04:47:56Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T04:47:56Z- handleDepth()- Depth_Keller msg: 1730868476.39 epoch sec; seqNo:1879; depth 50.00 2024-11-06T04:47:56Z- handleDVL() - DVL msg epochMillisec: 1730868476.41 2024-11-06T04:47:56Z- handleDvl() - RDI_Pathfinder msg: 1730868476.41 sec; seqNo:689; #beams:4 2024-11-06T04:47:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.27 , 33.81 , 37.04 , 35.68 2024-11-06T04:47:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.03 2024-11-06T04:47:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868476.44 epoch sec; seqNo:1879; 4061697.36 north; 594558.64 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T04:47:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868475.17 2024-11-06T04:47:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:56Z- waiting for 1730868479.17, time is 1730868476.44 2024-11-06T04:47:56Z- AHRS_M2 msg: 1730868476.78 epoch sec; seqNo:1752 2024-11-06T04:47:56Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T04:47:56Z- handleDepth()- Depth_Keller msg: 1730868476.79 epoch sec; seqNo:1880; depth 49.98 2024-11-06T04:47:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868476.82 epoch sec; seqNo:1880; 4061697.36 north; 594558.64 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T04:47:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868475.17 2024-11-06T04:47:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:56Z- waiting for 1730868479.17, time is 1730868476.82 2024-11-06T04:47:57Z- AHRS_M2 msg: 1730868477.19 epoch sec; seqNo:1753 2024-11-06T04:47:57Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T04:47:57Z- handleDepth()- Depth_Keller msg: 1730868477.19 epoch sec; seqNo:1881; depth 49.96 2024-11-06T04:47:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868477.20 epoch sec; seqNo:1881; 4061697.38 north; 594559.86 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:47:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868475.17 2024-11-06T04:47:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:57Z- waiting for 1730868479.17, time is 1730868477.20 2024-11-06T04:47:57Z- AHRS_M2 msg: 1730868477.59 epoch sec; seqNo:1754 2024-11-06T04:47:57Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T04:47:57Z- handleDepth()- Depth_Keller msg: 1730868477.59 epoch sec; seqNo:1882; depth 49.94 2024-11-06T04:47:57Z- handleDVL() - DVL msg epochMillisec: 1730868477.60 2024-11-06T04:47:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:57Z- handleDvl() - RDI_Pathfinder msg: 1730868477.60 sec; seqNo:690; #beams:0 2024-11-06T04:47:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.03 2024-11-06T04:47:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868477.60 epoch sec; seqNo:1882; 4061697.38 north; 594559.86 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T04:47:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868475.17 2024-11-06T04:47:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:57Z- waiting for 1730868479.17, time is 1730868477.60 2024-11-06T04:47:57Z- AHRS_M2 msg: 1730868477.99 epoch sec; seqNo:1755 2024-11-06T04:47:58Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T04:47:58Z- handleDepth()- Depth_Keller msg: 1730868478.01 epoch sec; seqNo:1883; depth 49.91 2024-11-06T04:47:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868478.03 epoch sec; seqNo:1883; 4061697.38 north; 594559.86 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:47:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868475.17 2024-11-06T04:47:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:58Z- waiting for 1730868479.17, time is 1730868478.03 2024-11-06T04:47:58Z- handleDepth()- Depth_Keller msg: 1730868478.42 epoch sec; seqNo:1884; depth 49.89 2024-11-06T04:47:58Z- handleDVL() - DVL msg epochMillisec: 1730868478.45 2024-11-06T04:47:58Z- handleDvl() - RDI_Pathfinder msg: 1730868478.45 sec; seqNo:691; #beams:4 2024-11-06T04:47:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.44 , 33.99 , 37.21 , 35.85 2024-11-06T04:47:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , -0.03 2024-11-06T04:47:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868478.48 epoch sec; seqNo:1884; 4061697.39 north; 594561.08 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:47:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868475.17 2024-11-06T04:47:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:58Z- waiting for 1730868479.17, time is 1730868478.48 2024-11-06T04:47:58Z- AHRS_M2 msg: 1730868478.80 epoch sec; seqNo:1756 2024-11-06T04:47:58Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T04:47:58Z- handleDepth()- Depth_Keller msg: 1730868478.81 epoch sec; seqNo:1885; depth 49.87 2024-11-06T04:47:58Z- handleDVL() - DVL msg epochMillisec: 1730868478.81 2024-11-06T04:47:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:47:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:47:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:47:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:47:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:47:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:47:58Z- handleDvl() - RDI_Pathfinder msg: 1730868478.81 sec; seqNo:692; #beams:0 2024-11-06T04:47:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:47:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , -0.03 2024-11-06T04:47:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868478.86 epoch sec; seqNo:1885; 4061697.39 north; 594561.08 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:47:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:47:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868475.17 2024-11-06T04:47:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:58Z- waiting for 1730868479.17, time is 1730868478.86 2024-11-06T04:47:59Z- AHRS_M2 msg: 1730868479.21 epoch sec; seqNo:1757 2024-11-06T04:47:59Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T04:47:59Z- handleDepth()- Depth_Keller msg: 1730868479.22 epoch sec; seqNo:1886; depth 49.86 2024-11-06T04:47:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868479.23 epoch sec; seqNo:1886; 4061697.77 north; 594561.08 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:47:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868475.17 2024-11-06T04:47:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:59Z- waiting for 1730868479.17, time is 1730868479.23 2024-11-06T04:47:59Z- LcmTrn::updateTrn() >>>> pose time 1730868479.23, meas time 1730868478.81 <<<< 2024-11-06T04:47:59Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868479.23 2024-11-06T04:47:59Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:47:59Z- TerrainNav::motionUpdate() - currEstimate = (4061698, 594561.08, 49.86) 2024-11-06T04:47:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:47:59Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:47:59Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:47:59Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:47:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:47:59Z- variances([0], [2], and psi_berg) = 0.380571 0.543252 0.000000 2024-11-06T04:47:59Z- publishEstimatesDecoder() - at 1730868479.23 sec 2024-11-06T04:47:59Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:47:59Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061641.34 594564.51 53.79 2024-11-06T04:47:59Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061641.45 594565.57 54.46 lat/lon: 36.70 -121.94 2024-11-06T04:47:59Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.38 0.54 1.27 0.00 2024-11-06T04:47:59Z- AHRS_M2 msg: 1730868479.61 epoch sec; seqNo:1758 2024-11-06T04:47:59Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T04:47:59Z- handleDepth()- Depth_Keller msg: 1730868479.62 epoch sec; seqNo:1887; depth 49.83 2024-11-06T04:47:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868479.62 epoch sec; seqNo:1887; 4061697.78 north; 594562.30 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:47:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:47:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868479.23 2024-11-06T04:47:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:47:59Z- waiting for 1730868483.23, time is 1730868479.62 2024-11-06T04:48:00Z- AHRS_M2 msg: 1730868480.01 epoch sec; seqNo:1759 2024-11-06T04:48:00Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T04:48:00Z- handleDepth()- Depth_Keller msg: 1730868480.02 epoch sec; seqNo:1888; depth 49.81 2024-11-06T04:48:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868480.03 epoch sec; seqNo:1888; 4061697.78 north; 594562.30 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:48:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868479.23 2024-11-06T04:48:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:00Z- waiting for 1730868483.23, time is 1730868480.03 2024-11-06T04:48:00Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:48:00Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:48:00Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:48:00Z- AHRS_M2 msg: 1730868480.41 epoch sec; seqNo:1760 2024-11-06T04:48:00Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T04:48:00Z- handleDepth()- Depth_Keller msg: 1730868480.44 epoch sec; seqNo:1889; depth 49.78 2024-11-06T04:48:00Z- handleDVL() - DVL msg epochMillisec: 1730868480.46 2024-11-06T04:48:00Z- handleDvl() - RDI_Pathfinder msg: 1730868480.46 sec; seqNo:693; #beams:4 2024-11-06T04:48:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.44 , 33.81 , 37.38 , 35.85 2024-11-06T04:48:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.07 2024-11-06T04:48:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868480.50 epoch sec; seqNo:1889; 4061697.78 north; 594562.30 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:48:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868479.23 2024-11-06T04:48:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:00Z- waiting for 1730868483.23, time is 1730868480.50 2024-11-06T04:48:00Z- AHRS_M2 msg: 1730868480.82 epoch sec; seqNo:1761 2024-11-06T04:48:00Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T04:48:00Z- handleDepth()- Depth_Keller msg: 1730868480.83 epoch sec; seqNo:1890; depth 49.76 2024-11-06T04:48:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868480.85 epoch sec; seqNo:1890; 4061697.78 north; 594562.30 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T04:48:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868479.23 2024-11-06T04:48:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:00Z- waiting for 1730868483.23, time is 1730868480.85 2024-11-06T04:48:01Z- AHRS_M2 msg: 1730868481.22 epoch sec; seqNo:1762 2024-11-06T04:48:01Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.51 psi 2024-11-06T04:48:01Z- handleDepth()- Depth_Keller msg: 1730868481.23 epoch sec; seqNo:1891; depth 49.72 2024-11-06T04:48:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868481.25 epoch sec; seqNo:1891; 4061697.80 north; 594563.52 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:48:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868479.23 2024-11-06T04:48:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:01Z- waiting for 1730868483.23, time is 1730868481.25 2024-11-06T04:48:01Z- AHRS_M2 msg: 1730868481.63 epoch sec; seqNo:1763 2024-11-06T04:48:01Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.51 psi 2024-11-06T04:48:01Z- handleDepth()- Depth_Keller msg: 1730868481.63 epoch sec; seqNo:1892; depth 49.72 2024-11-06T04:48:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868481.64 epoch sec; seqNo:1892; 4061697.80 north; 594563.52 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:48:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868479.23 2024-11-06T04:48:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:01Z- waiting for 1730868483.23, time is 1730868481.64 2024-11-06T04:48:02Z- AHRS_M2 msg: 1730868482.03 epoch sec; seqNo:1764 2024-11-06T04:48:02Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.51 psi 2024-11-06T04:48:02Z- handleDepth()- Depth_Keller msg: 1730868482.04 epoch sec; seqNo:1893; depth 49.70 2024-11-06T04:48:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868482.06 epoch sec; seqNo:1893; 4061697.80 north; 594563.52 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T04:48:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868479.23 2024-11-06T04:48:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:02Z- waiting for 1730868483.23, time is 1730868482.06 2024-11-06T04:48:02Z- AHRS_M2 msg: 1730868482.43 epoch sec; seqNo:1765 2024-11-06T04:48:02Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.50 psi 2024-11-06T04:48:02Z- handleDepth()- Depth_Keller msg: 1730868482.44 epoch sec; seqNo:1894; depth 49.68 2024-11-06T04:48:02Z- handleDVL() - DVL msg epochMillisec: 1730868482.46 2024-11-06T04:48:02Z- handleDvl() - RDI_Pathfinder msg: 1730868482.46 sec; seqNo:694; #beams:4 2024-11-06T04:48:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.44 , 33.81 , 37.04 , 35.85 2024-11-06T04:48:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.09 , -0.05 2024-11-06T04:48:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868482.49 epoch sec; seqNo:1894; 4061697.80 north; 594563.52 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T04:48:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868479.23 2024-11-06T04:48:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:02Z- waiting for 1730868483.23, time is 1730868482.49 2024-11-06T04:48:02Z- AHRS_M2 msg: 1730868482.85 epoch sec; seqNo:1766 2024-11-06T04:48:02Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.50 psi 2024-11-06T04:48:02Z- handleDepth()- Depth_Keller msg: 1730868482.86 epoch sec; seqNo:1895; depth 49.66 2024-11-06T04:48:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868482.88 epoch sec; seqNo:1895; 4061697.81 north; 594564.74 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:48:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868479.23 2024-11-06T04:48:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:03Z- waiting for 1730868483.23, time is 1730868482.88 2024-11-06T04:48:03Z- AHRS_M2 msg: 1730868483.24 epoch sec; seqNo:1767 2024-11-06T04:48:03Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T04:48:03Z- handleDepth()- Depth_Keller msg: 1730868483.25 epoch sec; seqNo:1896; depth 49.65 2024-11-06T04:48:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868483.28 epoch sec; seqNo:1896; 4061698.19 north; 594564.73 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:48:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868479.23 2024-11-06T04:48:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:03Z- waiting for 1730868483.23, time is 1730868483.28 2024-11-06T04:48:03Z- LcmTrn::updateTrn() >>>> pose time 1730868483.28, meas time 1730868482.46 <<<< 2024-11-06T04:48:03Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868483.28 2024-11-06T04:48:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868482.46 sec, ping # 0; waiting for INS data more recent than 1730868479.23... 2024-11-06T04:48:03Z- TerrainNav::motionUpdate() - currEstimate = (4061698, 594564.73, 49.65) 2024-11-06T04:48:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:48:03Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:48:03Z- TNavPF::Projecting Measurements 2024-11-06T04:48:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:48:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:48:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:48:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:48:03Z- TNavFilter::4 good beams. 2024-11-06T04:48:03Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:48:03Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:48:03Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:48:03Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:48:03Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:48:03Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:48:03Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:48:03Z- TerrainMapOctree: (vehN, vehE ) = (4061698.10, 594564.00). 2024-11-06T04:48:03Z- TerrainMapOctree: Min Distance = 193.11. 2024-11-06T04:48:03Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:48:03Z- Forcing Subcloud Comparison 2024-11-06T04:48:03Z- Weighting particles with subcloud comparison 2024-11-06T04:48:03Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:48:03Z- meanExpectedMeasDiff: -0.654833 delta_rms_squared: 2.393136 alpha: 0.991712 2024-11-06T04:48:03Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:48:03Z- meanExpectedMeasDiff: -0.049575 delta_rms_squared: 1.722569 alpha: 0.988523 2024-11-06T04:48:03Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:48:03Z- meanExpectedMeasDiff: 1.279624 delta_rms_squared: 2.257947 alpha: 0.991220 2024-11-06T04:48:03Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:48:03Z- meanExpectedMeasDiff: -1.530436 delta_rms_squared: 1.975624 alpha: 0.989978 2024-11-06T04:48:03Z- Subcloud NIS: 0.379150 2024-11-06T04:48:03Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:48:03Z- TNavPF:: sumWeights = 0.332246 2024-11-06T04:48:03Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:48:03Z- TNavPF::First Term of Map Covariance Matrix is 0.29 2024-11-06T04:48:03Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T04:48:03Z- TerrainNav::Resampling particle filter... 2024-11-06T04:48:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868482.46 sec. 2024-11-06T04:48:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:48:03Z- variances([0], [2], and psi_berg) = 0.370752 0.553704 0.000000 2024-11-06T04:48:03Z- publishEstimatesDecoder() - at 1730868483.28 sec 2024-11-06T04:48:03Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:48:03Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061641.73 594568.05 53.69 2024-11-06T04:48:03Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061641.89 594569.23 54.37 lat/lon: 36.70 -121.94 2024-11-06T04:48:03Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.37 0.55 0.72 0.00 2024-11-06T04:48:03Z- AHRS_M2 msg: 1730868483.65 epoch sec; seqNo:1768 2024-11-06T04:48:03Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.49 psi 2024-11-06T04:48:03Z- handleDepth()- Depth_Keller msg: 1730868483.67 epoch sec; seqNo:1897; depth 49.62 2024-11-06T04:48:03Z- handleDVL() - DVL msg epochMillisec: 1730868483.68 2024-11-06T04:48:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:03Z- handleDvl() - RDI_Pathfinder msg: 1730868483.68 sec; seqNo:695; #beams:0 2024-11-06T04:48:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.09 , -0.05 2024-11-06T04:48:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868483.69 epoch sec; seqNo:1897; 4061698.19 north; 594564.73 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:48:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868483.28 2024-11-06T04:48:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:03Z- waiting for 1730868487.28, time is 1730868483.69 2024-11-06T04:48:04Z- AHRS_M2 msg: 1730868484.06 epoch sec; seqNo:1769 2024-11-06T04:48:04Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.49 psi 2024-11-06T04:48:04Z- handleDepth()- Depth_Keller msg: 1730868484.07 epoch sec; seqNo:1898; depth 49.61 2024-11-06T04:48:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868484.13 epoch sec; seqNo:1898; 4061698.19 north; 594564.73 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:48:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868483.28 2024-11-06T04:48:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:04Z- waiting for 1730868487.28, time is 1730868484.13 2024-11-06T04:48:04Z- AHRS_M2 msg: 1730868484.46 epoch sec; seqNo:1770 2024-11-06T04:48:04Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.49 psi 2024-11-06T04:48:04Z- handleDepth()- Depth_Keller msg: 1730868484.46 epoch sec; seqNo:1899; depth 49.61 2024-11-06T04:48:04Z- handleDVL() - DVL msg epochMillisec: 1730868484.48 2024-11-06T04:48:04Z- handleDvl() - RDI_Pathfinder msg: 1730868484.48 sec; seqNo:696; #beams:4 2024-11-06T04:48:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.27 , 33.81 , 36.70 , 36.19 2024-11-06T04:48:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.08 , 0.02 2024-11-06T04:48:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868484.50 epoch sec; seqNo:1899; 4061698.20 north; 594565.96 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:48:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868483.28 2024-11-06T04:48:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:04Z- waiting for 1730868487.28, time is 1730868484.50 2024-11-06T04:48:04Z- AHRS_M2 msg: 1730868484.86 epoch sec; seqNo:1771 2024-11-06T04:48:04Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.49 psi 2024-11-06T04:48:04Z- handleDepth()- Depth_Keller msg: 1730868484.88 epoch sec; seqNo:1900; depth 49.60 2024-11-06T04:48:04Z- handleDVL() - DVL msg epochMillisec: 1730868484.88 2024-11-06T04:48:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:04Z- handleDvl() - RDI_Pathfinder msg: 1730868484.88 sec; seqNo:697; #beams:0 2024-11-06T04:48:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.08 , 0.02 2024-11-06T04:48:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868484.89 epoch sec; seqNo:1900; 4061698.20 north; 594565.96 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:48:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868483.28 2024-11-06T04:48:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:05Z- waiting for 1730868487.28, time is 1730868484.89 2024-11-06T04:48:05Z- AHRS_M2 msg: 1730868485.27 epoch sec; seqNo:1772 2024-11-06T04:48:05Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.49 psi 2024-11-06T04:48:05Z- handleDepth()- Depth_Keller msg: 1730868485.28 epoch sec; seqNo:1901; depth 49.59 2024-11-06T04:48:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868485.30 epoch sec; seqNo:1901; 4061698.20 north; 594565.96 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:48:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868483.28 2024-11-06T04:48:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:05Z- waiting for 1730868487.28, time is 1730868485.30 2024-11-06T04:48:05Z- AHRS_M2 msg: 1730868485.68 epoch sec; seqNo:1773 2024-11-06T04:48:05Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.48 psi 2024-11-06T04:48:05Z- handleDepth()- Depth_Keller msg: 1730868485.70 epoch sec; seqNo:1902; depth 49.60 2024-11-06T04:48:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868485.72 epoch sec; seqNo:1902; 4061698.58 north; 594565.95 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:48:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868483.28 2024-11-06T04:48:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:05Z- waiting for 1730868487.28, time is 1730868485.72 2024-11-06T04:48:06Z- AHRS_M2 msg: 1730868486.08 epoch sec; seqNo:1774 2024-11-06T04:48:06Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.48 psi 2024-11-06T04:48:06Z- handleDepth()- Depth_Keller msg: 1730868486.09 epoch sec; seqNo:1903; depth 49.59 2024-11-06T04:48:06Z- handleDVL() - DVL msg epochMillisec: 1730868486.10 2024-11-06T04:48:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:06Z- handleDvl() - RDI_Pathfinder msg: 1730868486.10 sec; seqNo:698; #beams:0 2024-11-06T04:48:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.08 , 0.02 2024-11-06T04:48:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868486.16 epoch sec; seqNo:1903; 4061698.59 north; 594567.17 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:48:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868483.28 2024-11-06T04:48:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:06Z- waiting for 1730868487.28, time is 1730868486.16 2024-11-06T04:48:06Z- AHRS_M2 msg: 1730868486.47 epoch sec; seqNo:1775 2024-11-06T04:48:06Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.48 psi 2024-11-06T04:48:06Z- handleDepth()- Depth_Keller msg: 1730868486.48 epoch sec; seqNo:1904; depth 49.57 2024-11-06T04:48:06Z- handleDVL() - DVL msg epochMillisec: 1730868486.51 2024-11-06T04:48:06Z- handleDvl() - RDI_Pathfinder msg: 1730868486.51 sec; seqNo:699; #beams:4 2024-11-06T04:48:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.27 , 33.99 , 36.87 , 36.53 2024-11-06T04:48:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , 0.07 2024-11-06T04:48:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868486.52 epoch sec; seqNo:1904; 4061698.59 north; 594567.17 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T04:48:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868483.28 2024-11-06T04:48:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:06Z- waiting for 1730868487.28, time is 1730868486.52 2024-11-06T04:48:06Z- AHRS_M2 msg: 1730868486.88 epoch sec; seqNo:1776 2024-11-06T04:48:06Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.48 psi 2024-11-06T04:48:06Z- handleDepth()- Depth_Keller msg: 1730868486.88 epoch sec; seqNo:1905; depth 49.58 2024-11-06T04:48:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868486.90 epoch sec; seqNo:1905; 4061698.59 north; 594567.17 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:48:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868483.28 2024-11-06T04:48:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:07Z- waiting for 1730868487.28, time is 1730868486.90 2024-11-06T04:48:07Z- AHRS_M2 msg: 1730868487.29 epoch sec; seqNo:1777 2024-11-06T04:48:07Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.48 psi 2024-11-06T04:48:07Z- handleDepth()- Depth_Keller msg: 1730868487.29 epoch sec; seqNo:1906; depth 49.57 2024-11-06T04:48:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868487.30 epoch sec; seqNo:1906; 4061698.59 north; 594567.17 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T04:48:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868483.28 2024-11-06T04:48:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:07Z- waiting for 1730868487.28, time is 1730868487.30 2024-11-06T04:48:07Z- LcmTrn::updateTrn() >>>> pose time 1730868487.30, meas time 1730868486.51 <<<< 2024-11-06T04:48:07Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868487.30 2024-11-06T04:48:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868486.51 sec, ping # 0; waiting for INS data more recent than 1730868483.28... 2024-11-06T04:48:07Z- TerrainNav::motionUpdate() - currEstimate = (4061699, 594567.17, 49.57) 2024-11-06T04:48:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:48:07Z- TerrainNav:: SubcloudNIS is 0.38, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:48:07Z- TNavPF::Projecting Measurements 2024-11-06T04:48:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:48:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:48:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:48:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:48:07Z- TNavFilter::4 good beams. 2024-11-06T04:48:07Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:48:07Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:48:07Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:48:07Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:48:07Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:48:07Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:48:07Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:48:07Z- TerrainMapOctree: (vehN, vehE ) = (4061698.51, 594566.69). 2024-11-06T04:48:07Z- TerrainMapOctree: Min Distance = 190.40. 2024-11-06T04:48:07Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:48:07Z- Forcing Subcloud Comparison 2024-11-06T04:48:07Z- Weighting particles with subcloud comparison 2024-11-06T04:48:07Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:48:07Z- meanExpectedMeasDiff: -0.105002 delta_rms_squared: 2.336613 alpha: 0.991513 2024-11-06T04:48:07Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:48:07Z- meanExpectedMeasDiff: 0.138160 delta_rms_squared: 1.116284 alpha: 0.982399 2024-11-06T04:48:07Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:48:07Z- meanExpectedMeasDiff: 1.371948 delta_rms_squared: 1.353123 alpha: 0.985435 2024-11-06T04:48:07Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:48:07Z- meanExpectedMeasDiff: -0.966444 delta_rms_squared: 1.856679 alpha: 0.989343 2024-11-06T04:48:07Z- Subcloud NIS: 0.365994 2024-11-06T04:48:07Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:48:07Z- TNavPF:: sumWeights = 0.410138 2024-11-06T04:48:07Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:48:07Z- TNavPF::First Term of Map Covariance Matrix is 0.45 2024-11-06T04:48:07Z- TNavPF::Calculated NIS Value : 0.19 numBeams normalized NIS: 0.05 2024-11-06T04:48:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868486.51 sec. 2024-11-06T04:48:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:48:07Z- variances([0], [2], and psi_berg) = 0.385203 0.560262 0.000000 2024-11-06T04:48:07Z- publishEstimatesDecoder() - at 1730868487.30 sec 2024-11-06T04:48:07Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:48:07Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061642.62 594572.28 53.47 2024-11-06T04:48:07Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061642.30 594571.68 54.26 lat/lon: 36.70 -121.94 2024-11-06T04:48:07Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.39 0.56 0.66 0.00 2024-11-06T04:48:07Z- AHRS_M2 msg: 1730868487.69 epoch sec; seqNo:1778 2024-11-06T04:48:07Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.49 psi 2024-11-06T04:48:07Z- handleDepth()- Depth_Keller msg: 1730868487.69 epoch sec; seqNo:1907; depth 49.57 2024-11-06T04:48:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868487.70 epoch sec; seqNo:1907; 4061698.61 north; 594568.39 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T04:48:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868487.30 2024-11-06T04:48:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:07Z- waiting for 1730868491.30, time is 1730868487.70 2024-11-06T04:48:08Z- AHRS_M2 msg: 1730868488.12 epoch sec; seqNo:1779 2024-11-06T04:48:08Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.49 psi 2024-11-06T04:48:08Z- handleDepth()- Depth_Keller msg: 1730868488.15 epoch sec; seqNo:1908; depth 49.56 2024-11-06T04:48:08Z- handleDVL() - DVL msg epochMillisec: 1730868488.17 2024-11-06T04:48:08Z- handleDvl() - RDI_Pathfinder msg: 1730868488.17 sec; seqNo:700; #beams:4 2024-11-06T04:48:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.61 , 33.99 , 36.70 , 36.87 2024-11-06T04:48:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.05 2024-11-06T04:48:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868488.20 epoch sec; seqNo:1908; 4061698.61 north; 594568.39 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:48:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868487.30 2024-11-06T04:48:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:08Z- waiting for 1730868491.30, time is 1730868488.20 2024-11-06T04:48:08Z- AHRS_M2 msg: 1730868488.50 epoch sec; seqNo:1780 2024-11-06T04:48:08Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.49 psi 2024-11-06T04:48:08Z- handleDepth()- Depth_Keller msg: 1730868488.51 epoch sec; seqNo:1909; depth 49.56 2024-11-06T04:48:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868488.52 epoch sec; seqNo:1909; 4061698.61 north; 594568.39 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:48:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868487.30 2024-11-06T04:48:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:08Z- waiting for 1730868491.30, time is 1730868488.52 2024-11-06T04:48:08Z- AHRS_M2 msg: 1730868488.90 epoch sec; seqNo:1781 2024-11-06T04:48:08Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.49 psi 2024-11-06T04:48:08Z- handleDepth()- Depth_Keller msg: 1730868488.91 epoch sec; seqNo:1910; depth 49.56 2024-11-06T04:48:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868488.93 epoch sec; seqNo:1910; 4061699.00 north; 594569.61 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:48:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868487.30 2024-11-06T04:48:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:09Z- waiting for 1730868491.30, time is 1730868488.93 2024-11-06T04:48:09Z- AHRS_M2 msg: 1730868489.30 epoch sec; seqNo:1782 2024-11-06T04:48:09Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:48:09Z- handleDepth()- Depth_Keller msg: 1730868489.31 epoch sec; seqNo:1911; depth 49.57 2024-11-06T04:48:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868489.32 epoch sec; seqNo:1911; 4061699.00 north; 594569.61 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T04:48:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868487.30 2024-11-06T04:48:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:09Z- waiting for 1730868491.30, time is 1730868489.32 2024-11-06T04:48:09Z- AHRS_M2 msg: 1730868489.72 epoch sec; seqNo:1783 2024-11-06T04:48:09Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:48:09Z- handleDepth()- Depth_Keller msg: 1730868489.72 epoch sec; seqNo:1912; depth 49.54 2024-11-06T04:48:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868489.73 epoch sec; seqNo:1912; 4061699.00 north; 594569.61 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T04:48:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868487.30 2024-11-06T04:48:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:09Z- waiting for 1730868491.30, time is 1730868489.73 2024-11-06T04:48:10Z- AHRS_M2 msg: 1730868490.15 epoch sec; seqNo:1784 2024-11-06T04:48:10Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:48:10Z- handleDepth()- Depth_Keller msg: 1730868490.16 epoch sec; seqNo:1913; depth 49.54 2024-11-06T04:48:10Z- handleDVL() - DVL msg epochMillisec: 1730868490.18 2024-11-06T04:48:10Z- handleDvl() - RDI_Pathfinder msg: 1730868490.18 sec; seqNo:701; #beams:4 2024-11-06T04:48:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.29 , 33.99 , 36.70 , 37.04 2024-11-06T04:48:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.00 2024-11-06T04:48:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868490.20 epoch sec; seqNo:1913; 4061699.01 north; 594570.83 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T04:48:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868487.30 2024-11-06T04:48:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:10Z- waiting for 1730868491.30, time is 1730868490.20 2024-11-06T04:48:10Z- AHRS_M2 msg: 1730868490.52 epoch sec; seqNo:1785 2024-11-06T04:48:10Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:48:10Z- handleDepth()- Depth_Keller msg: 1730868490.52 epoch sec; seqNo:1914; depth 49.55 2024-11-06T04:48:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868490.55 epoch sec; seqNo:1914; 4061699.01 north; 594570.83 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T04:48:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868487.30 2024-11-06T04:48:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:10Z- waiting for 1730868491.30, time is 1730868490.55 2024-11-06T04:48:10Z- AHRS_M2 msg: 1730868490.92 epoch sec; seqNo:1786 2024-11-06T04:48:10Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:48:10Z- handleDepth()- Depth_Keller msg: 1730868490.93 epoch sec; seqNo:1915; depth 49.56 2024-11-06T04:48:10Z- handleDVL() - DVL msg epochMillisec: 1730868490.93 2024-11-06T04:48:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:10Z- handleDvl() - RDI_Pathfinder msg: 1730868490.93 sec; seqNo:702; #beams:0 2024-11-06T04:48:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.00 2024-11-06T04:48:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868490.94 epoch sec; seqNo:1915; 4061699.01 north; 594570.83 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:48:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868487.30 2024-11-06T04:48:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:11Z- waiting for 1730868491.30, time is 1730868490.94 2024-11-06T04:48:11Z- AHRS_M2 msg: 1730868491.32 epoch sec; seqNo:1787 2024-11-06T04:48:11Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:48:11Z- handleDepth()- Depth_Keller msg: 1730868491.33 epoch sec; seqNo:1916; depth 49.54 2024-11-06T04:48:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868491.34 epoch sec; seqNo:1916; 4061699.01 north; 594570.83 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T04:48:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868487.30 2024-11-06T04:48:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:11Z- waiting for 1730868491.30, time is 1730868491.34 2024-11-06T04:48:11Z- LcmTrn::updateTrn() >>>> pose time 1730868491.34, meas time 1730868490.93 <<<< 2024-11-06T04:48:11Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868491.34 2024-11-06T04:48:11Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:48:11Z- TerrainNav::motionUpdate() - currEstimate = (4061699, 594570.83, 49.54) 2024-11-06T04:48:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:48:11Z- TerrainNav:: SubcloudNIS is 0.37, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:48:11Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:48:11Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:48:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:48:11Z- variances([0], [2], and psi_berg) = 0.387536 0.561958 0.000000 2024-11-06T04:48:11Z- publishEstimatesDecoder() - at 1730868491.34 sec 2024-11-06T04:48:11Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:48:11Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061643.01 594575.87 52.62 2024-11-06T04:48:11Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061642.71 594575.34 54.23 lat/lon: 36.70 -121.94 2024-11-06T04:48:11Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.39 0.56 0.91 0.00 2024-11-06T04:48:11Z- AHRS_M2 msg: 1730868491.73 epoch sec; seqNo:1788 2024-11-06T04:48:11Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.51 psi 2024-11-06T04:48:11Z- handleDepth()- Depth_Keller msg: 1730868491.74 epoch sec; seqNo:1917; depth 49.53 2024-11-06T04:48:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868491.75 epoch sec; seqNo:1917; 4061699.03 north; 594572.05 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T04:48:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868491.34 2024-11-06T04:48:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:11Z- waiting for 1730868495.34, time is 1730868491.75 2024-11-06T04:48:12Z- AHRS_M2 msg: 1730868492.16 epoch sec; seqNo:1789 2024-11-06T04:48:12Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.51 psi 2024-11-06T04:48:12Z- handleDepth()- Depth_Keller msg: 1730868492.16 epoch sec; seqNo:1918; depth 49.53 2024-11-06T04:48:12Z- handleDVL() - DVL msg epochMillisec: 1730868492.18 2024-11-06T04:48:12Z- handleDvl() - RDI_Pathfinder msg: 1730868492.18 sec; seqNo:703; #beams:4 2024-11-06T04:48:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.95 , 33.99 , 36.70 , 37.21 2024-11-06T04:48:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.04 2024-11-06T04:48:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868492.19 epoch sec; seqNo:1918; 4061699.03 north; 594572.05 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T04:48:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868491.34 2024-11-06T04:48:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:12Z- waiting for 1730868495.34, time is 1730868492.19 2024-11-06T04:48:12Z- AHRS_M2 msg: 1730868492.54 epoch sec; seqNo:1790 2024-11-06T04:48:12Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.51 psi 2024-11-06T04:48:12Z- handleDepth()- Depth_Keller msg: 1730868492.54 epoch sec; seqNo:1919; depth 49.53 2024-11-06T04:48:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868492.56 epoch sec; seqNo:1919; 4061699.03 north; 594572.05 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T04:48:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868491.34 2024-11-06T04:48:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:12Z- waiting for 1730868495.34, time is 1730868492.56 2024-11-06T04:48:12Z- AHRS_M2 msg: 1730868492.94 epoch sec; seqNo:1791 2024-11-06T04:48:12Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.51 psi 2024-11-06T04:48:12Z- handleDepth()- Depth_Keller msg: 1730868492.95 epoch sec; seqNo:1920; depth 49.52 2024-11-06T04:48:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868492.96 epoch sec; seqNo:1920; 4061699.03 north; 594572.05 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:48:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868491.34 2024-11-06T04:48:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:13Z- waiting for 1730868495.34, time is 1730868492.96 2024-11-06T04:48:13Z- AHRS_M2 msg: 1730868493.34 epoch sec; seqNo:1792 2024-11-06T04:48:13Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.51 psi 2024-11-06T04:48:13Z- handleDepth()- Depth_Keller msg: 1730868493.35 epoch sec; seqNo:1921; depth 49.51 2024-11-06T04:48:13Z- handleDVL() - DVL msg epochMillisec: 1730868493.35 2024-11-06T04:48:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:13Z- handleDvl() - RDI_Pathfinder msg: 1730868493.35 sec; seqNo:704; #beams:0 2024-11-06T04:48:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.04 2024-11-06T04:48:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868493.37 epoch sec; seqNo:1921; 4061699.04 north; 594573.27 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T04:48:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868491.34 2024-11-06T04:48:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:13Z- waiting for 1730868495.34, time is 1730868493.37 2024-11-06T04:48:13Z- AHRS_M2 msg: 1730868493.75 epoch sec; seqNo:1793 2024-11-06T04:48:13Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.51 psi 2024-11-06T04:48:13Z- handleDepth()- Depth_Keller msg: 1730868493.75 epoch sec; seqNo:1922; depth 49.51 2024-11-06T04:48:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868493.76 epoch sec; seqNo:1922; 4061699.42 north; 594573.26 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T04:48:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868491.34 2024-11-06T04:48:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:13Z- waiting for 1730868495.34, time is 1730868493.76 2024-11-06T04:48:14Z- AHRS_M2 msg: 1730868494.16 epoch sec; seqNo:1794 2024-11-06T04:48:14Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.51 psi 2024-11-06T04:48:14Z- handleDepth()- Depth_Keller msg: 1730868494.17 epoch sec; seqNo:1923; depth 49.48 2024-11-06T04:48:14Z- handleDVL() - DVL msg epochMillisec: 1730868494.19 2024-11-06T04:48:14Z- handleDvl() - RDI_Pathfinder msg: 1730868494.19 sec; seqNo:705; #beams:4 2024-11-06T04:48:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.95 , 33.99 , 36.36 , 37.21 2024-11-06T04:48:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , -0.03 2024-11-06T04:48:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868494.23 epoch sec; seqNo:1923; 4061699.42 north; 594573.26 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T04:48:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868491.34 2024-11-06T04:48:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:14Z- waiting for 1730868495.34, time is 1730868494.23 2024-11-06T04:48:14Z- AHRS_M2 msg: 1730868494.55 epoch sec; seqNo:1795 2024-11-06T04:48:14Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.51 psi 2024-11-06T04:48:14Z- handleDepth()- Depth_Keller msg: 1730868494.57 epoch sec; seqNo:1924; depth 49.49 2024-11-06T04:48:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868494.59 epoch sec; seqNo:1924; 4061699.43 north; 594574.49 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:48:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868491.34 2024-11-06T04:48:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:14Z- waiting for 1730868495.34, time is 1730868494.59 2024-11-06T04:48:14Z- AHRS_M2 msg: 1730868494.96 epoch sec; seqNo:1796 2024-11-06T04:48:14Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.51 psi 2024-11-06T04:48:14Z- handleDepth()- Depth_Keller msg: 1730868494.96 epoch sec; seqNo:1925; depth 49.47 2024-11-06T04:48:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868494.97 epoch sec; seqNo:1925; 4061699.43 north; 594574.49 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T04:48:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868491.34 2024-11-06T04:48:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:15Z- waiting for 1730868495.34, time is 1730868494.97 2024-11-06T04:48:15Z- AHRS_M2 msg: 1730868495.37 epoch sec; seqNo:1797 2024-11-06T04:48:15Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.51 psi 2024-11-06T04:48:15Z- handleDepth()- Depth_Keller msg: 1730868495.38 epoch sec; seqNo:1926; depth 49.49 2024-11-06T04:48:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868495.38 epoch sec; seqNo:1926; 4061699.43 north; 594574.49 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:48:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868491.34 2024-11-06T04:48:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:15Z- waiting for 1730868495.34, time is 1730868495.38 2024-11-06T04:48:15Z- LcmTrn::updateTrn() >>>> pose time 1730868495.38, meas time 1730868494.19 <<<< 2024-11-06T04:48:15Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868495.38 2024-11-06T04:48:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868494.19 sec, ping # 0; waiting for INS data more recent than 1730868491.34... 2024-11-06T04:48:15Z- TerrainNav::motionUpdate() - currEstimate = (4061699, 594574.49, 49.49) 2024-11-06T04:48:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:48:15Z- TerrainNav:: SubcloudNIS is 0.37, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:48:15Z- TNavPF::Projecting Measurements 2024-11-06T04:48:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:48:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:48:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:48:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:48:15Z- TNavFilter::4 good beams. 2024-11-06T04:48:15Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:48:15Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:48:15Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:48:15Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:48:15Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:48:15Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:48:15Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:48:15Z- TerrainMapOctree: (vehN, vehE ) = (4061699.31, 594573.40). 2024-11-06T04:48:15Z- TerrainMapOctree: Min Distance = 183.73. 2024-11-06T04:48:15Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:48:15Z- Forcing Subcloud Comparison 2024-11-06T04:48:15Z- Weighting particles with subcloud comparison 2024-11-06T04:48:15Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:48:15Z- meanExpectedMeasDiff: 0.546993 delta_rms_squared: 1.895340 alpha: 0.989558 2024-11-06T04:48:15Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:48:15Z- meanExpectedMeasDiff: 0.208429 delta_rms_squared: 1.321542 alpha: 0.985092 2024-11-06T04:48:15Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:48:15Z- meanExpectedMeasDiff: 1.134318 delta_rms_squared: 1.535084 alpha: 0.987139 2024-11-06T04:48:15Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:48:15Z- meanExpectedMeasDiff: -0.000741 delta_rms_squared: 1.606961 alpha: 0.987707 2024-11-06T04:48:15Z- Subcloud NIS: 0.353233 2024-11-06T04:48:15Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:48:15Z- TNavPF:: sumWeights = 0.543296 2024-11-06T04:48:15Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:48:15Z- TNavPF::First Term of Map Covariance Matrix is 0.40 2024-11-06T04:48:15Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T04:48:15Z- TerrainNav::Resampling particle filter... 2024-11-06T04:48:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868494.19 sec. 2024-11-06T04:48:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:48:15Z- variances([0], [2], and psi_berg) = 0.392695 0.556492 0.000000 2024-11-06T04:48:15Z- publishEstimatesDecoder() - at 1730868495.38 sec 2024-11-06T04:48:15Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:48:15Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061643.07 594577.83 54.04 2024-11-06T04:48:15Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061643.14 594579.01 53.92 lat/lon: 36.70 -121.94 2024-11-06T04:48:15Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.39 0.56 0.70 0.00 2024-11-06T04:48:15Z- AHRS_M2 msg: 1730868495.77 epoch sec; seqNo:1798 2024-11-06T04:48:15Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.51 psi 2024-11-06T04:48:15Z- handleDepth()- Depth_Keller msg: 1730868495.78 epoch sec; seqNo:1927; depth 49.48 2024-11-06T04:48:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868495.80 epoch sec; seqNo:1927; 4061699.43 north; 594574.49 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T04:48:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868495.38 2024-11-06T04:48:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:15Z- waiting for 1730868499.38, time is 1730868495.80 2024-11-06T04:48:16Z- AHRS_M2 msg: 1730868496.17 epoch sec; seqNo:1799 2024-11-06T04:48:16Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.51 psi 2024-11-06T04:48:16Z- handleDepth()- Depth_Keller msg: 1730868496.18 epoch sec; seqNo:1928; depth 49.49 2024-11-06T04:48:16Z- handleDVL() - DVL msg epochMillisec: 1730868496.21 2024-11-06T04:48:16Z- handleDvl() - RDI_Pathfinder msg: 1730868496.21 sec; seqNo:706; #beams:4 2024-11-06T04:48:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.95 , 33.99 , 36.36 , 37.21 2024-11-06T04:48:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.07 , 0.02 2024-11-06T04:48:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868496.22 epoch sec; seqNo:1928; 4061699.45 north; 594575.71 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:48:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868495.38 2024-11-06T04:48:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:16Z- waiting for 1730868499.38, time is 1730868496.22 2024-11-06T04:48:16Z- handleDepth()- Depth_Keller msg: 1730868496.60 epoch sec; seqNo:1929; depth 49.48 2024-11-06T04:48:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868496.61 epoch sec; seqNo:1929; 4061699.45 north; 594575.71 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T04:48:16Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:48:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868495.38 2024-11-06T04:48:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:16Z- waiting for 1730868499.38, time is 1730868496.61 2024-11-06T04:48:16Z- AHRS_M2 msg: 1730868496.98 epoch sec; seqNo:1800 2024-11-06T04:48:16Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T04:48:16Z- handleDepth()- Depth_Keller msg: 1730868496.98 epoch sec; seqNo:1930; depth 49.49 2024-11-06T04:48:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868497.00 epoch sec; seqNo:1930; 4061699.83 north; 594575.70 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:48:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868495.38 2024-11-06T04:48:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:17Z- waiting for 1730868499.38, time is 1730868497.00 2024-11-06T04:48:17Z- AHRS_M2 msg: 1730868497.39 epoch sec; seqNo:1801 2024-11-06T04:48:17Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T04:48:17Z- handleDepth()- Depth_Keller msg: 1730868497.40 epoch sec; seqNo:1931; depth 49.47 2024-11-06T04:48:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868497.41 epoch sec; seqNo:1931; 4061699.83 north; 594575.70 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T04:48:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868495.38 2024-11-06T04:48:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:17Z- waiting for 1730868499.38, time is 1730868497.41 2024-11-06T04:48:17Z- AHRS_M2 msg: 1730868497.79 epoch sec; seqNo:1802 2024-11-06T04:48:17Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T04:48:17Z- handleDepth()- Depth_Keller msg: 1730868497.80 epoch sec; seqNo:1932; depth 49.48 2024-11-06T04:48:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868497.82 epoch sec; seqNo:1932; 4061699.84 north; 594576.92 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T04:48:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868495.38 2024-11-06T04:48:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:17Z- waiting for 1730868499.38, time is 1730868497.82 2024-11-06T04:48:18Z- handleDepth()- Depth_Keller msg: 1730868498.22 epoch sec; seqNo:1933; depth 49.48 2024-11-06T04:48:18Z- handleDVL() - DVL msg epochMillisec: 1730868498.24 2024-11-06T04:48:18Z- handleDvl() - RDI_Pathfinder msg: 1730868498.24 sec; seqNo:707; #beams:4 2024-11-06T04:48:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.78 , 33.99 , 36.02 , 37.38 2024-11-06T04:48:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , 0.05 2024-11-06T04:48:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868498.25 epoch sec; seqNo:1933; 4061699.84 north; 594576.92 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T04:48:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868495.38 2024-11-06T04:48:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:18Z- waiting for 1730868499.38, time is 1730868498.25 2024-11-06T04:48:18Z- AHRS_M2 msg: 1730868498.60 epoch sec; seqNo:1803 2024-11-06T04:48:18Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T04:48:18Z- handleDepth()- Depth_Keller msg: 1730868498.61 epoch sec; seqNo:1934; depth 49.50 2024-11-06T04:48:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868498.62 epoch sec; seqNo:1934; 4061699.84 north; 594576.92 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T04:48:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868495.38 2024-11-06T04:48:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:18Z- waiting for 1730868499.38, time is 1730868498.62 2024-11-06T04:48:19Z- AHRS_M2 msg: 1730868499.00 epoch sec; seqNo:1804 2024-11-06T04:48:19Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T04:48:19Z- handleDepth()- Depth_Keller msg: 1730868499.03 epoch sec; seqNo:1935; depth 49.50 2024-11-06T04:48:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868499.04 epoch sec; seqNo:1935; 4061699.84 north; 594576.92 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T04:48:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868495.38 2024-11-06T04:48:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:19Z- waiting for 1730868499.38, time is 1730868499.04 2024-11-06T04:48:19Z- AHRS_M2 msg: 1730868499.40 epoch sec; seqNo:1805 2024-11-06T04:48:19Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T04:48:19Z- handleDepth()- Depth_Keller msg: 1730868499.41 epoch sec; seqNo:1936; depth 49.49 2024-11-06T04:48:19Z- handleDVL() - DVL msg epochMillisec: 1730868499.41 2024-11-06T04:48:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:19Z- handleDvl() - RDI_Pathfinder msg: 1730868499.41 sec; seqNo:708; #beams:0 2024-11-06T04:48:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , 0.05 2024-11-06T04:48:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868499.42 epoch sec; seqNo:1936; 4061699.85 north; 594578.14 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:48:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868495.38 2024-11-06T04:48:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:19Z- waiting for 1730868499.38, time is 1730868499.42 2024-11-06T04:48:19Z- LcmTrn::updateTrn() >>>> pose time 1730868499.42, meas time 1730868499.41 <<<< 2024-11-06T04:48:19Z- LcmTrn::updateTrn() equating pose 1730868499.41 and meas times 1730868499.41 2024-11-06T04:48:19Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868499.41 2024-11-06T04:48:19Z- TerrainNav::motionUpdate() - currEstimate = (4061700, 594578.14, 49.49) 2024-11-06T04:48:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:48:19Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:48:19Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:48:19Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:48:19Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:48:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:48:19Z- variances([0], [2], and psi_berg) = 0.394606 0.560279 0.000000 2024-11-06T04:48:19Z- publishEstimatesDecoder() - at 1730868499.41 sec 2024-11-06T04:48:19Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:48:19Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061643.56 594581.53 54.18 2024-11-06T04:48:19Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061643.56 594582.66 53.92 lat/lon: 36.70 -121.94 2024-11-06T04:48:19Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.39 0.56 0.96 0.00 2024-11-06T04:48:19Z- AHRS_M2 msg: 1730868499.81 epoch sec; seqNo:1806 2024-11-06T04:48:19Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T04:48:19Z- handleDepth()- Depth_Keller msg: 1730868499.82 epoch sec; seqNo:1937; depth 49.51 2024-11-06T04:48:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868499.84 epoch sec; seqNo:1937; 4061699.85 north; 594578.14 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T04:48:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868499.41 2024-11-06T04:48:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:19Z- waiting for 1730868503.41, time is 1730868499.84 2024-11-06T04:48:20Z- AHRS_M2 msg: 1730868500.21 epoch sec; seqNo:1807 2024-11-06T04:48:20Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T04:48:20Z- handleDepth()- Depth_Keller msg: 1730868500.22 epoch sec; seqNo:1938; depth 49.49 2024-11-06T04:48:20Z- handleDVL() - DVL msg epochMillisec: 1730868500.24 2024-11-06T04:48:20Z- handleDvl() - RDI_Pathfinder msg: 1730868500.24 sec; seqNo:709; #beams:4 2024-11-06T04:48:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.78 , 34.15 , 36.02 , 37.55 2024-11-06T04:48:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.03 2024-11-06T04:48:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868500.26 epoch sec; seqNo:1938; 4061700.23 north; 594578.14 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:48:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868499.41 2024-11-06T04:48:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:20Z- waiting for 1730868503.41, time is 1730868500.26 2024-11-06T04:48:20Z- AHRS_M2 msg: 1730868500.61 epoch sec; seqNo:1808 2024-11-06T04:48:20Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T04:48:20Z- handleDepth()- Depth_Keller msg: 1730868500.63 epoch sec; seqNo:1939; depth 49.51 2024-11-06T04:48:20Z- handleDVL() - DVL msg epochMillisec: 1730868500.63 2024-11-06T04:48:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:20Z- handleDvl() - RDI_Pathfinder msg: 1730868500.63 sec; seqNo:710; #beams:0 2024-11-06T04:48:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.03 2024-11-06T04:48:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868500.65 epoch sec; seqNo:1939; 4061700.25 north; 594579.36 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T04:48:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868499.41 2024-11-06T04:48:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:20Z- waiting for 1730868503.41, time is 1730868500.65 2024-11-06T04:48:21Z- AHRS_M2 msg: 1730868501.02 epoch sec; seqNo:1809 2024-11-06T04:48:21Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T04:48:21Z- handleDepth()- Depth_Keller msg: 1730868501.03 epoch sec; seqNo:1940; depth 49.51 2024-11-06T04:48:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868501.06 epoch sec; seqNo:1940; 4061700.25 north; 594579.36 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T04:48:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868499.41 2024-11-06T04:48:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:21Z- waiting for 1730868503.41, time is 1730868501.06 2024-11-06T04:48:21Z- AHRS_M2 msg: 1730868501.43 epoch sec; seqNo:1810 2024-11-06T04:48:21Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T04:48:21Z- handleDepth()- Depth_Keller msg: 1730868501.43 epoch sec; seqNo:1941; depth 49.50 2024-11-06T04:48:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868501.44 epoch sec; seqNo:1941; 4061700.25 north; 594579.36 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T04:48:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868499.41 2024-11-06T04:48:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:21Z- waiting for 1730868503.41, time is 1730868501.44 2024-11-06T04:48:21Z- AHRS_M2 msg: 1730868501.83 epoch sec; seqNo:1811 2024-11-06T04:48:21Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T04:48:21Z- handleDepth()- Depth_Keller msg: 1730868501.84 epoch sec; seqNo:1942; depth 49.52 2024-11-06T04:48:21Z- handleDVL() - DVL msg epochMillisec: 1730868501.84 2024-11-06T04:48:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:21Z- handleDvl() - RDI_Pathfinder msg: 1730868501.84 sec; seqNo:711; #beams:0 2024-11-06T04:48:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.03 2024-11-06T04:48:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868501.85 epoch sec; seqNo:1942; 4061700.25 north; 594579.36 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:48:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868499.41 2024-11-06T04:48:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:21Z- waiting for 1730868503.41, time is 1730868501.85 2024-11-06T04:48:22Z- AHRS_M2 msg: 1730868502.23 epoch sec; seqNo:1812 2024-11-06T04:48:22Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T04:48:22Z- handleDepth()- Depth_Keller msg: 1730868502.24 epoch sec; seqNo:1943; depth 49.51 2024-11-06T04:48:22Z- handleDVL() - DVL msg epochMillisec: 1730868502.27 2024-11-06T04:48:22Z- handleDvl() - RDI_Pathfinder msg: 1730868502.27 sec; seqNo:712; #beams:4 2024-11-06T04:48:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.95 , 33.99 , 36.02 , 37.38 2024-11-06T04:48:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , -0.01 2024-11-06T04:48:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868502.28 epoch sec; seqNo:1943; 4061700.26 north; 594580.58 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T04:48:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868499.41 2024-11-06T04:48:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:22Z- waiting for 1730868503.41, time is 1730868502.28 2024-11-06T04:48:22Z- AHRS_M2 msg: 1730868502.63 epoch sec; seqNo:1813 2024-11-06T04:48:22Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T04:48:22Z- handleDepth()- Depth_Keller msg: 1730868502.64 epoch sec; seqNo:1944; depth 49.50 2024-11-06T04:48:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868502.65 epoch sec; seqNo:1944; 4061700.26 north; 594580.58 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T04:48:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868499.41 2024-11-06T04:48:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:22Z- waiting for 1730868503.41, time is 1730868502.65 2024-11-06T04:48:23Z- AHRS_M2 msg: 1730868503.04 epoch sec; seqNo:1814 2024-11-06T04:48:23Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:48:23Z- handleDepth()- Depth_Keller msg: 1730868503.04 epoch sec; seqNo:1945; depth 49.52 2024-11-06T04:48:23Z- handleDVL() - DVL msg epochMillisec: 1730868503.05 2024-11-06T04:48:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:23Z- handleDvl() - RDI_Pathfinder msg: 1730868503.05 sec; seqNo:713; #beams:0 2024-11-06T04:48:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , -0.01 2024-11-06T04:48:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868503.06 epoch sec; seqNo:1945; 4061700.26 north; 594580.58 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:48:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868499.41 2024-11-06T04:48:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:23Z- waiting for 1730868503.41, time is 1730868503.06 2024-11-06T04:48:23Z- AHRS_M2 msg: 1730868503.45 epoch sec; seqNo:1815 2024-11-06T04:48:23Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:48:23Z- handleDepth()- Depth_Keller msg: 1730868503.46 epoch sec; seqNo:1946; depth 49.53 2024-11-06T04:48:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868503.47 epoch sec; seqNo:1946; 4061700.27 north; 594581.80 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T04:48:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868499.41 2024-11-06T04:48:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:23Z- waiting for 1730868503.41, time is 1730868503.47 2024-11-06T04:48:23Z- LcmTrn::updateTrn() >>>> pose time 1730868503.47, meas time 1730868503.05 <<<< 2024-11-06T04:48:23Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868503.47 2024-11-06T04:48:23Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:48:23Z- TerrainNav::motionUpdate() - currEstimate = (4061700, 594581.80, 49.53) 2024-11-06T04:48:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:48:23Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:48:23Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:48:23Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:48:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:48:23Z- variances([0], [2], and psi_berg) = 0.397627 0.563139 0.000000 2024-11-06T04:48:23Z- publishEstimatesDecoder() - at 1730868503.47 sec 2024-11-06T04:48:23Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:48:23Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061644.01 594585.19 53.91 2024-11-06T04:48:23Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061643.98 594586.32 53.96 lat/lon: 36.70 -121.94 2024-11-06T04:48:23Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.40 0.56 1.21 0.00 2024-11-06T04:48:23Z- AHRS_M2 msg: 1730868503.85 epoch sec; seqNo:1816 2024-11-06T04:48:23Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:48:23Z- handleDepth()- Depth_Keller msg: 1730868503.85 epoch sec; seqNo:1947; depth 49.52 2024-11-06T04:48:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868503.86 epoch sec; seqNo:1947; 4061700.27 north; 594581.80 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:48:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868503.47 2024-11-06T04:48:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:24Z- waiting for 1730868507.47, time is 1730868503.86 2024-11-06T04:48:24Z- AHRS_M2 msg: 1730868504.25 epoch sec; seqNo:1817 2024-11-06T04:48:24Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:48:24Z- handleDepth()- Depth_Keller msg: 1730868504.26 epoch sec; seqNo:1948; depth 49.52 2024-11-06T04:48:24Z- handleDVL() - DVL msg epochMillisec: 1730868504.28 2024-11-06T04:48:24Z- handleDvl() - RDI_Pathfinder msg: 1730868504.28 sec; seqNo:714; #beams:4 2024-11-06T04:48:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.95 , 33.81 , 35.85 , 37.55 2024-11-06T04:48:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.02 2024-11-06T04:48:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868504.31 epoch sec; seqNo:1948; 4061700.27 north; 594581.80 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:48:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868503.47 2024-11-06T04:48:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:24Z- waiting for 1730868507.47, time is 1730868504.31 2024-11-06T04:48:24Z- AHRS_M2 msg: 1730868504.66 epoch sec; seqNo:1818 2024-11-06T04:48:24Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:48:24Z- handleDepth()- Depth_Keller msg: 1730868504.66 epoch sec; seqNo:1949; depth 49.53 2024-11-06T04:48:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868504.68 epoch sec; seqNo:1949; 4061700.65 north; 594581.79 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T04:48:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868503.47 2024-11-06T04:48:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:24Z- waiting for 1730868507.47, time is 1730868504.68 2024-11-06T04:48:25Z- AHRS_M2 msg: 1730868505.06 epoch sec; seqNo:1819 2024-11-06T04:48:25Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:48:25Z- handleDepth()- Depth_Keller msg: 1730868505.06 epoch sec; seqNo:1950; depth 49.52 2024-11-06T04:48:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868505.08 epoch sec; seqNo:1950; 4061700.66 north; 594583.01 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:48:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868503.47 2024-11-06T04:48:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:25Z- waiting for 1730868507.47, time is 1730868505.08 2024-11-06T04:48:25Z- AHRS_M2 msg: 1730868505.46 epoch sec; seqNo:1820 2024-11-06T04:48:25Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:48:25Z- handleDepth()- Depth_Keller msg: 1730868505.47 epoch sec; seqNo:1951; depth 49.53 2024-11-06T04:48:25Z- handleDVL() - DVL msg epochMillisec: 1730868505.47 2024-11-06T04:48:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:25Z- handleDvl() - RDI_Pathfinder msg: 1730868505.47 sec; seqNo:715; #beams:0 2024-11-06T04:48:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.02 2024-11-06T04:48:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868505.51 epoch sec; seqNo:1951; 4061700.66 north; 594583.01 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T04:48:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868503.47 2024-11-06T04:48:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:25Z- waiting for 1730868507.47, time is 1730868505.51 2024-11-06T04:48:25Z- AHRS_M2 msg: 1730868505.87 epoch sec; seqNo:1821 2024-11-06T04:48:25Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:48:25Z- handleDepth()- Depth_Keller msg: 1730868505.87 epoch sec; seqNo:1952; depth 49.53 2024-11-06T04:48:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868505.88 epoch sec; seqNo:1952; 4061700.66 north; 594583.01 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T04:48:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868503.47 2024-11-06T04:48:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:26Z- waiting for 1730868507.47, time is 1730868505.88 2024-11-06T04:48:26Z- AHRS_M2 msg: 1730868506.27 epoch sec; seqNo:1822 2024-11-06T04:48:26Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:48:26Z- handleDepth()- Depth_Keller msg: 1730868506.28 epoch sec; seqNo:1953; depth 49.54 2024-11-06T04:48:26Z- handleDVL() - DVL msg epochMillisec: 1730868506.30 2024-11-06T04:48:26Z- handleDvl() - RDI_Pathfinder msg: 1730868506.30 sec; seqNo:716; #beams:4 2024-11-06T04:48:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.61 , 33.65 , 35.51 , 37.55 2024-11-06T04:48:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , 0.01 2024-11-06T04:48:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868506.32 epoch sec; seqNo:1953; 4061700.66 north; 594583.01 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T04:48:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868503.47 2024-11-06T04:48:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:26Z- waiting for 1730868507.47, time is 1730868506.32 2024-11-06T04:48:26Z- AHRS_M2 msg: 1730868506.67 epoch sec; seqNo:1823 2024-11-06T04:48:26Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:48:26Z- handleDepth()- Depth_Keller msg: 1730868506.69 epoch sec; seqNo:1954; depth 49.53 2024-11-06T04:48:26Z- handleDVL() - DVL msg epochMillisec: 1730868506.69 2024-11-06T04:48:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:26Z- handleDvl() - RDI_Pathfinder msg: 1730868506.69 sec; seqNo:717; #beams:0 2024-11-06T04:48:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , 0.01 2024-11-06T04:48:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868506.71 epoch sec; seqNo:1954; 4061700.68 north; 594584.24 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T04:48:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868503.47 2024-11-06T04:48:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:26Z- waiting for 1730868507.47, time is 1730868506.71 2024-11-06T04:48:27Z- AHRS_M2 msg: 1730868507.08 epoch sec; seqNo:1824 2024-11-06T04:48:27Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:48:27Z- handleDepth()- Depth_Keller msg: 1730868507.09 epoch sec; seqNo:1955; depth 49.54 2024-11-06T04:48:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868507.10 epoch sec; seqNo:1955; 4061700.68 north; 594584.24 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T04:48:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868503.47 2024-11-06T04:48:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:27Z- waiting for 1730868507.47, time is 1730868507.10 2024-11-06T04:48:27Z- handleDepth()- Depth_Keller msg: 1730868507.51 epoch sec; seqNo:1956; depth 49.55 2024-11-06T04:48:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868507.52 epoch sec; seqNo:1956; 4061700.68 north; 594584.24 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T04:48:27Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:48:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868503.47 2024-11-06T04:48:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:27Z- waiting for 1730868507.47, time is 1730868507.52 2024-11-06T04:48:27Z- LcmTrn::updateTrn() >>>> pose time 1730868507.52, meas time 1730868506.69 <<<< 2024-11-06T04:48:27Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868507.52 2024-11-06T04:48:27Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:48:27Z- TerrainNav::motionUpdate() - currEstimate = (4061701, 594584.24, 49.55) 2024-11-06T04:48:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:48:27Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:48:27Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:48:27Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:48:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:48:27Z- variances([0], [2], and psi_berg) = 0.398621 0.563915 0.000000 2024-11-06T04:48:27Z- publishEstimatesDecoder() - at 1730868507.52 sec 2024-11-06T04:48:27Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:48:27Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061644.37 594587.65 53.83 2024-11-06T04:48:27Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061644.38 594588.76 53.97 lat/lon: 36.70 -121.94 2024-11-06T04:48:27Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.40 0.56 1.47 0.00 2024-11-06T04:48:27Z- AHRS_M2 msg: 1730868507.89 epoch sec; seqNo:1825 2024-11-06T04:48:27Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:48:27Z- handleDepth()- Depth_Keller msg: 1730868507.89 epoch sec; seqNo:1957; depth 49.55 2024-11-06T04:48:27Z- handleDVL() - DVL msg epochMillisec: 1730868507.90 2024-11-06T04:48:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:27Z- handleDvl() - RDI_Pathfinder msg: 1730868507.90 sec; seqNo:718; #beams:0 2024-11-06T04:48:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , 0.01 2024-11-06T04:48:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868507.92 epoch sec; seqNo:1957; 4061700.69 north; 594585.46 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T04:48:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868507.52 2024-11-06T04:48:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:28Z- waiting for 1730868511.52, time is 1730868507.92 2024-11-06T04:48:28Z- AHRS_M2 msg: 1730868508.29 epoch sec; seqNo:1826 2024-11-06T04:48:28Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:48:28Z- handleDepth()- Depth_Keller msg: 1730868508.30 epoch sec; seqNo:1958; depth 49.56 2024-11-06T04:48:28Z- handleDVL() - DVL msg epochMillisec: 1730868508.33 2024-11-06T04:48:28Z- handleDvl() - RDI_Pathfinder msg: 1730868508.33 sec; seqNo:719; #beams:4 2024-11-06T04:48:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.78 , 33.65 , 35.51 , 37.55 2024-11-06T04:48:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.03 2024-11-06T04:48:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868508.34 epoch sec; seqNo:1958; 4061700.69 north; 594585.46 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:48:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868507.52 2024-11-06T04:48:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:28Z- waiting for 1730868511.52, time is 1730868508.34 2024-11-06T04:48:28Z- AHRS_M2 msg: 1730868508.70 epoch sec; seqNo:1827 2024-11-06T04:48:28Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:48:28Z- handleDepth()- Depth_Keller msg: 1730868508.71 epoch sec; seqNo:1959; depth 49.56 2024-11-06T04:48:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868508.73 epoch sec; seqNo:1959; 4061701.07 north; 594585.45 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:48:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868507.52 2024-11-06T04:48:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:28Z- waiting for 1730868511.52, time is 1730868508.73 2024-11-06T04:48:29Z- AHRS_M2 msg: 1730868509.10 epoch sec; seqNo:1828 2024-11-06T04:48:29Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:48:29Z- handleDepth()- Depth_Keller msg: 1730868509.11 epoch sec; seqNo:1960; depth 49.56 2024-11-06T04:48:29Z- handleDVL() - DVL msg epochMillisec: 1730868509.12 2024-11-06T04:48:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:29Z- handleDvl() - RDI_Pathfinder msg: 1730868509.12 sec; seqNo:720; #beams:0 2024-11-06T04:48:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.03 2024-11-06T04:48:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868509.13 epoch sec; seqNo:1960; 4061701.07 north; 594585.45 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:48:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868507.52 2024-11-06T04:48:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:29Z- waiting for 1730868511.52, time is 1730868509.13 2024-11-06T04:48:29Z- AHRS_M2 msg: 1730868509.50 epoch sec; seqNo:1829 2024-11-06T04:48:29Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:48:29Z- handleDepth()- Depth_Keller msg: 1730868509.51 epoch sec; seqNo:1961; depth 49.57 2024-11-06T04:48:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868509.55 epoch sec; seqNo:1961; 4061701.08 north; 594586.67 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T04:48:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868507.52 2024-11-06T04:48:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:29Z- waiting for 1730868511.52, time is 1730868509.55 2024-11-06T04:48:29Z- AHRS_M2 msg: 1730868509.91 epoch sec; seqNo:1830 2024-11-06T04:48:29Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:48:29Z- handleDepth()- Depth_Keller msg: 1730868509.91 epoch sec; seqNo:1962; depth 49.58 2024-11-06T04:48:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868509.92 epoch sec; seqNo:1962; 4061701.08 north; 594586.67 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:48:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868507.52 2024-11-06T04:48:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:30Z- waiting for 1730868511.52, time is 1730868509.92 2024-11-06T04:48:30Z- AHRS_M2 msg: 1730868510.31 epoch sec; seqNo:1831 2024-11-06T04:48:30Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:48:30Z- handleDepth()- Depth_Keller msg: 1730868510.32 epoch sec; seqNo:1963; depth 49.58 2024-11-06T04:48:30Z- handleDVL() - DVL msg epochMillisec: 1730868510.34 2024-11-06T04:48:30Z- handleDvl() - RDI_Pathfinder msg: 1730868510.34 sec; seqNo:721; #beams:4 2024-11-06T04:48:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.44 , 33.65 , 35.51 , 37.55 2024-11-06T04:48:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , 0.02 2024-11-06T04:48:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868510.36 epoch sec; seqNo:1963; 4061701.08 north; 594586.67 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:48:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868507.52 2024-11-06T04:48:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:30Z- waiting for 1730868511.52, time is 1730868510.36 2024-11-06T04:48:30Z- AHRS_M2 msg: 1730868510.75 epoch sec; seqNo:1832 2024-11-06T04:48:30Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:48:30Z- handleDepth()- Depth_Keller msg: 1730868510.76 epoch sec; seqNo:1964; depth 49.59 2024-11-06T04:48:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868510.77 epoch sec; seqNo:1964; 4061701.08 north; 594586.67 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:48:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868507.52 2024-11-06T04:48:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:30Z- waiting for 1730868511.52, time is 1730868510.77 2024-11-06T04:48:31Z- AHRS_M2 msg: 1730868511.13 epoch sec; seqNo:1833 2024-11-06T04:48:31Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:48:31Z- handleDepth()- Depth_Keller msg: 1730868511.13 epoch sec; seqNo:1965; depth 49.59 2024-11-06T04:48:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868511.15 epoch sec; seqNo:1965; 4061701.10 north; 594587.89 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:48:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868507.52 2024-11-06T04:48:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:31Z- waiting for 1730868511.52, time is 1730868511.15 2024-11-06T04:48:31Z- AHRS_M2 msg: 1730868511.53 epoch sec; seqNo:1834 2024-11-06T04:48:31Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:48:31Z- handleDepth()- Depth_Keller msg: 1730868511.54 epoch sec; seqNo:1966; depth 49.61 2024-11-06T04:48:31Z- handleDVL() - DVL msg epochMillisec: 1730868511.54 2024-11-06T04:48:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:31Z- handleDvl() - RDI_Pathfinder msg: 1730868511.54 sec; seqNo:722; #beams:0 2024-11-06T04:48:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , 0.02 2024-11-06T04:48:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868511.55 epoch sec; seqNo:1966; 4061701.10 north; 594587.89 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:48:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868507.52 2024-11-06T04:48:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:31Z- waiting for 1730868511.52, time is 1730868511.55 2024-11-06T04:48:31Z- LcmTrn::updateTrn() >>>> pose time 1730868511.55, meas time 1730868511.54 <<<< 2024-11-06T04:48:31Z- LcmTrn::updateTrn() equating pose 1730868511.54 and meas times 1730868511.54 2024-11-06T04:48:31Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868511.54 2024-11-06T04:48:31Z- TerrainNav::motionUpdate() - currEstimate = (4061701, 594587.89, 49.61) 2024-11-06T04:48:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:48:31Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:48:31Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:48:31Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:48:31Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:48:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:48:31Z- variances([0], [2], and psi_berg) = 0.400489 0.566093 0.000000 2024-11-06T04:48:31Z- publishEstimatesDecoder() - at 1730868511.54 sec 2024-11-06T04:48:31Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:48:31Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061644.78 594591.30 54.47 2024-11-06T04:48:31Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061644.80 594592.41 54.03 lat/lon: 36.70 -121.94 2024-11-06T04:48:31Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.40 0.57 1.74 0.00 2024-11-06T04:48:31Z- AHRS_M2 msg: 1730868511.93 epoch sec; seqNo:1835 2024-11-06T04:48:31Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:48:31Z- handleDepth()- Depth_Keller msg: 1730868511.93 epoch sec; seqNo:1967; depth 49.62 2024-11-06T04:48:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868511.96 epoch sec; seqNo:1967; 4061701.10 north; 594587.89 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:48:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868511.54 2024-11-06T04:48:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:32Z- waiting for 1730868515.54, time is 1730868511.96 2024-11-06T04:48:32Z- AHRS_M2 msg: 1730868512.33 epoch sec; seqNo:1836 2024-11-06T04:48:32Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:48:32Z- handleDepth()- Depth_Keller msg: 1730868512.34 epoch sec; seqNo:1968; depth 49.63 2024-11-06T04:48:32Z- handleDVL() - DVL msg epochMillisec: 1730868512.37 2024-11-06T04:48:32Z- handleDvl() - RDI_Pathfinder msg: 1730868512.37 sec; seqNo:723; #beams:4 2024-11-06T04:48:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.44 , 33.65 , 35.34 , 37.55 2024-11-06T04:48:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , -0.01 2024-11-06T04:48:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868512.38 epoch sec; seqNo:1968; 4061701.11 north; 594589.11 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:48:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868511.54 2024-11-06T04:48:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:32Z- waiting for 1730868515.54, time is 1730868512.38 2024-11-06T04:48:32Z- AHRS_M2 msg: 1730868512.74 epoch sec; seqNo:1837 2024-11-06T04:48:32Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:48:32Z- handleDepth()- Depth_Keller msg: 1730868512.75 epoch sec; seqNo:1969; depth 49.63 2024-11-06T04:48:32Z- handleDVL() - DVL msg epochMillisec: 1730868512.76 2024-11-06T04:48:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:32Z- handleDvl() - RDI_Pathfinder msg: 1730868512.76 sec; seqNo:724; #beams:0 2024-11-06T04:48:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , -0.01 2024-11-06T04:48:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868512.78 epoch sec; seqNo:1969; 4061701.49 north; 594589.11 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:48:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868511.54 2024-11-06T04:48:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:32Z- waiting for 1730868515.54, time is 1730868512.78 2024-11-06T04:48:33Z- AHRS_M2 msg: 1730868513.14 epoch sec; seqNo:1838 2024-11-06T04:48:33Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:48:33Z- handleDepth()- Depth_Keller msg: 1730868513.14 epoch sec; seqNo:1970; depth 49.63 2024-11-06T04:48:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868513.16 epoch sec; seqNo:1970; 4061701.49 north; 594589.11 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:48:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868511.54 2024-11-06T04:48:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:33Z- waiting for 1730868515.54, time is 1730868513.16 2024-11-06T04:48:33Z- AHRS_M2 msg: 1730868513.54 epoch sec; seqNo:1839 2024-11-06T04:48:33Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.53 psi 2024-11-06T04:48:33Z- handleDepth()- Depth_Keller msg: 1730868513.55 epoch sec; seqNo:1971; depth 49.64 2024-11-06T04:48:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868513.56 epoch sec; seqNo:1971; 4061701.49 north; 594589.11 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:48:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868511.54 2024-11-06T04:48:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:33Z- waiting for 1730868515.54, time is 1730868513.56 2024-11-06T04:48:33Z- AHRS_M2 msg: 1730868513.95 epoch sec; seqNo:1840 2024-11-06T04:48:33Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:48:33Z- handleDepth()- Depth_Keller msg: 1730868513.95 epoch sec; seqNo:1972; depth 49.65 2024-11-06T04:48:33Z- handleDVL() - DVL msg epochMillisec: 1730868513.96 2024-11-06T04:48:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:33Z- handleDvl() - RDI_Pathfinder msg: 1730868513.96 sec; seqNo:725; #beams:0 2024-11-06T04:48:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , -0.01 2024-11-06T04:48:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868513.96 epoch sec; seqNo:1972; 4061701.50 north; 594590.33 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:48:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868511.54 2024-11-06T04:48:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:34Z- waiting for 1730868515.54, time is 1730868513.96 2024-11-06T04:48:34Z- AHRS_M2 msg: 1730868514.35 epoch sec; seqNo:1841 2024-11-06T04:48:34Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:48:34Z- handleDepth()- Depth_Keller msg: 1730868514.36 epoch sec; seqNo:1973; depth 49.65 2024-11-06T04:48:34Z- handleDVL() - DVL msg epochMillisec: 1730868514.38 2024-11-06T04:48:34Z- handleDvl() - RDI_Pathfinder msg: 1730868514.38 sec; seqNo:726; #beams:4 2024-11-06T04:48:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.27 , 33.47 , 35.17 , 37.72 2024-11-06T04:48:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.07 , -0.01 2024-11-06T04:48:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868514.40 epoch sec; seqNo:1973; 4061701.50 north; 594590.33 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:48:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868511.54 2024-11-06T04:48:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:34Z- waiting for 1730868515.54, time is 1730868514.40 2024-11-06T04:48:34Z- AHRS_M2 msg: 1730868514.76 epoch sec; seqNo:1842 2024-11-06T04:48:34Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:48:34Z- handleDepth()- Depth_Keller msg: 1730868514.77 epoch sec; seqNo:1974; depth 49.67 2024-11-06T04:48:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868514.78 epoch sec; seqNo:1974; 4061701.50 north; 594590.33 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:48:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868511.54 2024-11-06T04:48:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:34Z- waiting for 1730868515.54, time is 1730868514.78 2024-11-06T04:48:35Z- handleDepth()- Depth_Keller msg: 1730868515.18 epoch sec; seqNo:1975; depth 49.66 2024-11-06T04:48:35Z- handleDVL() - DVL msg epochMillisec: 1730868515.18 2024-11-06T04:48:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:35Z- handleDvl() - RDI_Pathfinder msg: 1730868515.18 sec; seqNo:727; #beams:0 2024-11-06T04:48:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.07 , -0.01 2024-11-06T04:48:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868515.19 epoch sec; seqNo:1975; 4061701.52 north; 594591.55 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:48:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868511.54 2024-11-06T04:48:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:35Z- waiting for 1730868515.54, time is 1730868515.19 2024-11-06T04:48:35Z- AHRS_M2 msg: 1730868515.56 epoch sec; seqNo:1843 2024-11-06T04:48:35Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:48:35Z- handleDepth()- Depth_Keller msg: 1730868515.57 epoch sec; seqNo:1976; depth 49.66 2024-11-06T04:48:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868515.58 epoch sec; seqNo:1976; 4061701.52 north; 594591.55 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:48:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868511.54 2024-11-06T04:48:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:35Z- waiting for 1730868515.54, time is 1730868515.58 2024-11-06T04:48:35Z- LcmTrn::updateTrn() >>>> pose time 1730868515.58, meas time 1730868515.18 <<<< 2024-11-06T04:48:35Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868515.58 2024-11-06T04:48:35Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:48:35Z- TerrainNav::motionUpdate() - currEstimate = (4061702, 594591.55, 49.66) 2024-11-06T04:48:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:48:35Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:48:35Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:48:35Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:48:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:48:35Z- variances([0], [2], and psi_berg) = 0.402242 0.567686 0.000000 2024-11-06T04:48:35Z- publishEstimatesDecoder() - at 1730868515.58 sec 2024-11-06T04:48:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:48:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061645.25 594594.97 54.72 2024-11-06T04:48:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061645.22 594596.07 54.07 lat/lon: 36.70 -121.94 2024-11-06T04:48:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.40 0.57 2.00 0.00 2024-11-06T04:48:35Z- AHRS_M2 msg: 1730868515.97 epoch sec; seqNo:1844 2024-11-06T04:48:35Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:48:35Z- handleDepth()- Depth_Keller msg: 1730868515.97 epoch sec; seqNo:1977; depth 49.66 2024-11-06T04:48:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868515.99 epoch sec; seqNo:1977; 4061701.90 north; 594591.54 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:48:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868515.58 2024-11-06T04:48:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:36Z- waiting for 1730868519.58, time is 1730868515.99 2024-11-06T04:48:36Z- AHRS_M2 msg: 1730868516.37 epoch sec; seqNo:1845 2024-11-06T04:48:36Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:48:36Z- handleDepth()- Depth_Keller msg: 1730868516.38 epoch sec; seqNo:1978; depth 49.68 2024-11-06T04:48:36Z- handleDVL() - DVL msg epochMillisec: 1730868516.40 2024-11-06T04:48:36Z- handleDvl() - RDI_Pathfinder msg: 1730868516.40 sec; seqNo:728; #beams:4 2024-11-06T04:48:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.27 , 33.65 , 35.00 , 37.55 2024-11-06T04:48:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.06 , 0.01 2024-11-06T04:48:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868516.41 epoch sec; seqNo:1978; 4061701.90 north; 594591.54 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T04:48:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868515.58 2024-11-06T04:48:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:36Z- waiting for 1730868519.58, time is 1730868516.41 2024-11-06T04:48:36Z- AHRS_M2 msg: 1730868516.78 epoch sec; seqNo:1846 2024-11-06T04:48:36Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:48:36Z- handleDepth()- Depth_Keller msg: 1730868516.79 epoch sec; seqNo:1979; depth 49.69 2024-11-06T04:48:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868516.82 epoch sec; seqNo:1979; 4061701.91 north; 594592.76 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T04:48:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868515.58 2024-11-06T04:48:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:36Z- waiting for 1730868519.58, time is 1730868516.82 2024-11-06T04:48:37Z- AHRS_M2 msg: 1730868517.18 epoch sec; seqNo:1847 2024-11-06T04:48:37Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:48:37Z- handleDepth()- Depth_Keller msg: 1730868517.19 epoch sec; seqNo:1980; depth 49.68 2024-11-06T04:48:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868517.19 epoch sec; seqNo:1980; 4061701.91 north; 594592.76 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T04:48:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868515.58 2024-11-06T04:48:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:37Z- waiting for 1730868519.58, time is 1730868517.19 2024-11-06T04:48:37Z- AHRS_M2 msg: 1730868517.58 epoch sec; seqNo:1848 2024-11-06T04:48:37Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:48:37Z- handleDepth()- Depth_Keller msg: 1730868517.59 epoch sec; seqNo:1981; depth 49.71 2024-11-06T04:48:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868517.60 epoch sec; seqNo:1981; 4061701.91 north; 594592.76 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T04:48:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868515.58 2024-11-06T04:48:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:37Z- waiting for 1730868519.58, time is 1730868517.60 2024-11-06T04:48:38Z- AHRS_M2 msg: 1730868517.99 epoch sec; seqNo:1849 2024-11-06T04:48:38Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:48:38Z- handleDepth()- Depth_Keller msg: 1730868518.00 epoch sec; seqNo:1982; depth 49.70 2024-11-06T04:48:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868518.02 epoch sec; seqNo:1982; 4061701.91 north; 594592.76 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T04:48:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868515.58 2024-11-06T04:48:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:38Z- waiting for 1730868519.58, time is 1730868518.02 2024-11-06T04:48:38Z- AHRS_M2 msg: 1730868518.39 epoch sec; seqNo:1850 2024-11-06T04:48:38Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:48:38Z- handleDepth()- Depth_Keller msg: 1730868518.41 epoch sec; seqNo:1983; depth 49.72 2024-11-06T04:48:38Z- handleDVL() - DVL msg epochMillisec: 1730868518.43 2024-11-06T04:48:38Z- handleDvl() - RDI_Pathfinder msg: 1730868518.43 sec; seqNo:729; #beams:4 2024-11-06T04:48:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.11 , 33.48 , 35.18 , 37.73 2024-11-06T04:48:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , 0.01 2024-11-06T04:48:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868518.44 epoch sec; seqNo:1983; 4061701.92 north; 594593.99 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:48:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868515.58 2024-11-06T04:48:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:38Z- waiting for 1730868519.58, time is 1730868518.44 2024-11-06T04:48:38Z- AHRS_M2 msg: 1730868518.80 epoch sec; seqNo:1851 2024-11-06T04:48:38Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:48:38Z- handleDepth()- Depth_Keller msg: 1730868518.80 epoch sec; seqNo:1984; depth 49.74 2024-11-06T04:48:38Z- handleDVL() - DVL msg epochMillisec: 1730868518.81 2024-11-06T04:48:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:38Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:38Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:38Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:38Z- handleDvl() - RDI_Pathfinder msg: 1730868518.81 sec; seqNo:730; #beams:0 2024-11-06T04:48:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , 0.01 2024-11-06T04:48:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868518.83 epoch sec; seqNo:1984; 4061701.92 north; 594593.99 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T04:48:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868515.58 2024-11-06T04:48:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:38Z- waiting for 1730868519.58, time is 1730868518.83 2024-11-06T04:48:39Z- AHRS_M2 msg: 1730868519.20 epoch sec; seqNo:1852 2024-11-06T04:48:39Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:48:39Z- handleDepth()- Depth_Keller msg: 1730868519.20 epoch sec; seqNo:1985; depth 49.74 2024-11-06T04:48:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868519.23 epoch sec; seqNo:1985; 4061701.92 north; 594593.99 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T04:48:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868515.58 2024-11-06T04:48:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:39Z- waiting for 1730868519.58, time is 1730868519.23 2024-11-06T04:48:39Z- AHRS_M2 msg: 1730868519.60 epoch sec; seqNo:1853 2024-11-06T04:48:39Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:48:39Z- handleDepth()- Depth_Keller msg: 1730868519.61 epoch sec; seqNo:1986; depth 49.75 2024-11-06T04:48:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868519.62 epoch sec; seqNo:1986; 4061701.94 north; 594595.21 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:48:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868515.58 2024-11-06T04:48:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:39Z- waiting for 1730868519.58, time is 1730868519.62 2024-11-06T04:48:39Z- LcmTrn::updateTrn() >>>> pose time 1730868519.62, meas time 1730868518.81 <<<< 2024-11-06T04:48:39Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868519.62 2024-11-06T04:48:39Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:48:39Z- TerrainNav::motionUpdate() - currEstimate = (4061702, 594595.21, 49.75) 2024-11-06T04:48:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:48:39Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:48:39Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:48:39Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:48:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:48:39Z- variances([0], [2], and psi_berg) = 0.403658 0.569450 0.000000 2024-11-06T04:48:39Z- publishEstimatesDecoder() - at 1730868519.62 sec 2024-11-06T04:48:39Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:48:39Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061645.67 594598.65 54.02 2024-11-06T04:48:39Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061645.64 594599.73 54.17 lat/lon: 36.70 -121.94 2024-11-06T04:48:39Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.40 0.57 2.25 0.00 2024-11-06T04:48:40Z- AHRS_M2 msg: 1730868520.01 epoch sec; seqNo:1854 2024-11-06T04:48:40Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.53 psi 2024-11-06T04:48:40Z- handleDepth()- Depth_Keller msg: 1730868520.02 epoch sec; seqNo:1987; depth 49.76 2024-11-06T04:48:40Z- handleDVL() - DVL msg epochMillisec: 1730868520.02 2024-11-06T04:48:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:40Z- handleDvl() - RDI_Pathfinder msg: 1730868520.02 sec; seqNo:731; #beams:0 2024-11-06T04:48:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , 0.01 2024-11-06T04:48:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868520.03 epoch sec; seqNo:1987; 4061701.94 north; 594595.21 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T04:48:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868519.62 2024-11-06T04:48:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:40Z- waiting for 1730868523.62, time is 1730868520.03 2024-11-06T04:48:40Z- AHRS_M2 msg: 1730868520.41 epoch sec; seqNo:1855 2024-11-06T04:48:40Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:40Z- handleDepth()- Depth_Keller msg: 1730868520.42 epoch sec; seqNo:1988; depth 49.75 2024-11-06T04:48:40Z- handleDVL() - DVL msg epochMillisec: 1730868520.44 2024-11-06T04:48:40Z- handleDvl() - RDI_Pathfinder msg: 1730868520.44 sec; seqNo:732; #beams:4 2024-11-06T04:48:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.93 , 33.31 , 35.00 , 37.55 2024-11-06T04:48:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.00 2024-11-06T04:48:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868520.45 epoch sec; seqNo:1988; 4061701.94 north; 594595.21 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:48:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868519.62 2024-11-06T04:48:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:40Z- waiting for 1730868523.62, time is 1730868520.45 2024-11-06T04:48:40Z- AHRS_M2 msg: 1730868520.82 epoch sec; seqNo:1856 2024-11-06T04:48:40Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:40Z- handleDepth()- Depth_Keller msg: 1730868520.83 epoch sec; seqNo:1989; depth 49.78 2024-11-06T04:48:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868520.85 epoch sec; seqNo:1989; 4061702.31 north; 594595.20 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:48:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868519.62 2024-11-06T04:48:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:40Z- waiting for 1730868523.62, time is 1730868520.85 2024-11-06T04:48:41Z- AHRS_M2 msg: 1730868521.22 epoch sec; seqNo:1857 2024-11-06T04:48:41Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:41Z- handleDepth()- Depth_Keller msg: 1730868521.22 epoch sec; seqNo:1990; depth 49.80 2024-11-06T04:48:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868521.23 epoch sec; seqNo:1990; 4061702.33 north; 594596.42 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T04:48:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868519.62 2024-11-06T04:48:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:41Z- waiting for 1730868523.62, time is 1730868521.23 2024-11-06T04:48:41Z- AHRS_M2 msg: 1730868521.63 epoch sec; seqNo:1858 2024-11-06T04:48:41Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:41Z- handleDepth()- Depth_Keller msg: 1730868521.64 epoch sec; seqNo:1991; depth 49.81 2024-11-06T04:48:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868521.65 epoch sec; seqNo:1991; 4061702.33 north; 594596.42 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:48:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868519.62 2024-11-06T04:48:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:41Z- waiting for 1730868523.62, time is 1730868521.65 2024-11-06T04:48:42Z- AHRS_M2 msg: 1730868522.03 epoch sec; seqNo:1859 2024-11-06T04:48:42Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:42Z- handleDepth()- Depth_Keller msg: 1730868522.04 epoch sec; seqNo:1992; depth 49.82 2024-11-06T04:48:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868522.06 epoch sec; seqNo:1992; 4061702.33 north; 594596.42 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:48:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868519.62 2024-11-06T04:48:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:42Z- waiting for 1730868523.62, time is 1730868522.06 2024-11-06T04:48:42Z- AHRS_M2 msg: 1730868522.43 epoch sec; seqNo:1860 2024-11-06T04:48:42Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:42Z- handleDepth()- Depth_Keller msg: 1730868522.44 epoch sec; seqNo:1993; depth 49.82 2024-11-06T04:48:42Z- handleDVL() - DVL msg epochMillisec: 1730868522.46 2024-11-06T04:48:42Z- handleDvl() - RDI_Pathfinder msg: 1730868522.46 sec; seqNo:733; #beams:4 2024-11-06T04:48:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.93 , 33.31 , 34.83 , 37.55 2024-11-06T04:48:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , -0.00 2024-11-06T04:48:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868522.47 epoch sec; seqNo:1993; 4061702.34 north; 594597.64 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:48:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868519.62 2024-11-06T04:48:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:42Z- waiting for 1730868523.62, time is 1730868522.47 2024-11-06T04:48:42Z- AHRS_M2 msg: 1730868522.84 epoch sec; seqNo:1861 2024-11-06T04:48:42Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:42Z- handleDepth()- Depth_Keller msg: 1730868522.85 epoch sec; seqNo:1994; depth 49.85 2024-11-06T04:48:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868522.86 epoch sec; seqNo:1994; 4061702.34 north; 594597.64 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:48:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868519.62 2024-11-06T04:48:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:43Z- waiting for 1730868523.62, time is 1730868522.86 2024-11-06T04:48:43Z- AHRS_M2 msg: 1730868523.24 epoch sec; seqNo:1862 2024-11-06T04:48:43Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:43Z- handleDepth()- Depth_Keller msg: 1730868523.24 epoch sec; seqNo:1995; depth 49.86 2024-11-06T04:48:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868523.26 epoch sec; seqNo:1995; 4061702.34 north; 594597.64 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:48:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868519.62 2024-11-06T04:48:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:43Z- waiting for 1730868523.62, time is 1730868523.26 2024-11-06T04:48:43Z- AHRS_M2 msg: 1730868523.65 epoch sec; seqNo:1863 2024-11-06T04:48:43Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:43Z- handleDepth()- Depth_Keller msg: 1730868523.65 epoch sec; seqNo:1996; depth 49.86 2024-11-06T04:48:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868523.66 epoch sec; seqNo:1996; 4061702.34 north; 594597.64 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:48:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868519.62 2024-11-06T04:48:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:43Z- waiting for 1730868523.62, time is 1730868523.66 2024-11-06T04:48:43Z- LcmTrn::updateTrn() >>>> pose time 1730868523.66, meas time 1730868522.46 <<<< 2024-11-06T04:48:43Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868523.66 2024-11-06T04:48:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868522.46 sec, ping # 0; waiting for INS data more recent than 1730868519.62... 2024-11-06T04:48:43Z- TerrainNav::motionUpdate() - currEstimate = (4061702, 594597.64, 49.86) 2024-11-06T04:48:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:48:43Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:48:43Z- TNavPF::Projecting Measurements 2024-11-06T04:48:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:48:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:48:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:48:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:48:43Z- TNavFilter::4 good beams. 2024-11-06T04:48:43Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:48:43Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:48:43Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:48:43Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:48:43Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:48:43Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:48:43Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:48:43Z- TerrainMapOctree: (vehN, vehE ) = (4061702.22, 594596.92). 2024-11-06T04:48:43Z- TerrainMapOctree: Min Distance = 160.37. 2024-11-06T04:48:43Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:48:43Z- Forcing Subcloud Comparison 2024-11-06T04:48:43Z- Weighting particles with subcloud comparison 2024-11-06T04:48:43Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:48:43Z- meanExpectedMeasDiff: 0.943929 delta_rms_squared: 4.110317 alpha: 0.995158 2024-11-06T04:48:43Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:48:43Z- meanExpectedMeasDiff: 0.573135 delta_rms_squared: 2.887450 alpha: 0.993121 2024-11-06T04:48:43Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:48:43Z- meanExpectedMeasDiff: 1.720417 delta_rms_squared: 2.511168 alpha: 0.992099 2024-11-06T04:48:43Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:48:43Z- meanExpectedMeasDiff: -0.256914 delta_rms_squared: 3.903929 alpha: 0.994903 2024-11-06T04:48:43Z- Subcloud NIS: 0.359485 2024-11-06T04:48:43Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:48:43Z- TNavPF:: sumWeights = 0.332326 2024-11-06T04:48:43Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:48:43Z- TNavPF::First Term of Map Covariance Matrix is 0.33 2024-11-06T04:48:43Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.03 2024-11-06T04:48:43Z- TerrainNav::Resampling particle filter... 2024-11-06T04:48:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868522.46 sec. 2024-11-06T04:48:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:48:43Z- variances([0], [2], and psi_berg) = 0.417268 0.540208 0.000000 2024-11-06T04:48:43Z- publishEstimatesDecoder() - at 1730868523.66 sec 2024-11-06T04:48:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:48:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061646.10 594601.06 54.75 2024-11-06T04:48:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061646.06 594602.08 53.84 lat/lon: 36.70 -121.94 2024-11-06T04:48:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.42 0.54 0.83 0.00 2024-11-06T04:48:44Z- AHRS_M2 msg: 1730868524.05 epoch sec; seqNo:1864 2024-11-06T04:48:44Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:44Z- handleDepth()- Depth_Keller msg: 1730868524.05 epoch sec; seqNo:1997; depth 49.87 2024-11-06T04:48:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868524.06 epoch sec; seqNo:1997; 4061702.35 north; 594598.86 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:48:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868523.66 2024-11-06T04:48:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:44Z- waiting for 1730868527.66, time is 1730868524.06 2024-11-06T04:48:44Z- AHRS_M2 msg: 1730868524.45 epoch sec; seqNo:1865 2024-11-06T04:48:44Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:44Z- handleDepth()- Depth_Keller msg: 1730868524.46 epoch sec; seqNo:1998; depth 49.89 2024-11-06T04:48:44Z- handleDVL() - DVL msg epochMillisec: 1730868524.48 2024-11-06T04:48:44Z- handleDvl() - RDI_Pathfinder msg: 1730868524.48 sec; seqNo:734; #beams:4 2024-11-06T04:48:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.10 , 33.31 , 34.49 , 37.72 2024-11-06T04:48:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.06 , 0.02 2024-11-06T04:48:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868524.50 epoch sec; seqNo:1998; 4061702.35 north; 594598.86 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:48:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868523.66 2024-11-06T04:48:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:44Z- waiting for 1730868527.66, time is 1730868524.50 2024-11-06T04:48:44Z- AHRS_M2 msg: 1730868524.86 epoch sec; seqNo:1866 2024-11-06T04:48:44Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:44Z- handleDepth()- Depth_Keller msg: 1730868524.87 epoch sec; seqNo:1999; depth 49.90 2024-11-06T04:48:44Z- handleDVL() - DVL msg epochMillisec: 1730868524.88 2024-11-06T04:48:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:44Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:44Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:44Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:44Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:44Z- handleDvl() - RDI_Pathfinder msg: 1730868524.88 sec; seqNo:735; #beams:0 2024-11-06T04:48:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.06 , 0.02 2024-11-06T04:48:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868524.89 epoch sec; seqNo:1999; 4061702.35 north; 594598.86 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:48:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868523.66 2024-11-06T04:48:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:45Z- waiting for 1730868527.66, time is 1730868524.89 2024-11-06T04:48:45Z- AHRS_M2 msg: 1730868525.26 epoch sec; seqNo:1867 2024-11-06T04:48:45Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:45Z- handleDepth()- Depth_Keller msg: 1730868525.27 epoch sec; seqNo:2000; depth 49.92 2024-11-06T04:48:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868525.28 epoch sec; seqNo:2000; 4061702.35 north; 594598.86 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:48:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868523.66 2024-11-06T04:48:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:45Z- waiting for 1730868527.66, time is 1730868525.28 2024-11-06T04:48:45Z- AHRS_M2 msg: 1730868525.67 epoch sec; seqNo:1868 2024-11-06T04:48:45Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:45Z- handleDepth()- Depth_Keller msg: 1730868525.67 epoch sec; seqNo:2001; depth 49.92 2024-11-06T04:48:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868525.69 epoch sec; seqNo:2001; 4061702.37 north; 594600.08 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:48:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868523.66 2024-11-06T04:48:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:45Z- waiting for 1730868527.66, time is 1730868525.69 2024-11-06T04:48:46Z- AHRS_M2 msg: 1730868526.08 epoch sec; seqNo:1869 2024-11-06T04:48:46Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:46Z- handleDepth()- Depth_Keller msg: 1730868526.09 epoch sec; seqNo:2002; depth 49.93 2024-11-06T04:48:46Z- handleDVL() - DVL msg epochMillisec: 1730868526.11 2024-11-06T04:48:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:46Z- handleDvl() - RDI_Pathfinder msg: 1730868526.11 sec; seqNo:736; #beams:0 2024-11-06T04:48:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.06 , 0.02 2024-11-06T04:48:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868526.15 epoch sec; seqNo:2002; 4061702.75 north; 594600.08 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T04:48:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868523.66 2024-11-06T04:48:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:46Z- waiting for 1730868527.66, time is 1730868526.15 2024-11-06T04:48:46Z- AHRS_M2 msg: 1730868526.47 epoch sec; seqNo:1870 2024-11-06T04:48:46Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:46Z- handleDepth()- Depth_Keller msg: 1730868526.48 epoch sec; seqNo:2003; depth 49.94 2024-11-06T04:48:46Z- handleDVL() - DVL msg epochMillisec: 1730868526.50 2024-11-06T04:48:46Z- handleDvl() - RDI_Pathfinder msg: 1730868526.50 sec; seqNo:737; #beams:4 2024-11-06T04:48:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.76 , 33.13 , 34.66 , 37.38 2024-11-06T04:48:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.04 2024-11-06T04:48:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868526.53 epoch sec; seqNo:2003; 4061702.75 north; 594600.08 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T04:48:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868523.66 2024-11-06T04:48:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:46Z- waiting for 1730868527.66, time is 1730868526.53 2024-11-06T04:48:46Z- AHRS_M2 msg: 1730868526.88 epoch sec; seqNo:1871 2024-11-06T04:48:46Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:46Z- handleDepth()- Depth_Keller msg: 1730868526.89 epoch sec; seqNo:2004; depth 49.95 2024-11-06T04:48:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868526.92 epoch sec; seqNo:2004; 4061702.76 north; 594601.30 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T04:48:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868523.66 2024-11-06T04:48:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:47Z- waiting for 1730868527.66, time is 1730868526.92 2024-11-06T04:48:47Z- AHRS_M2 msg: 1730868527.28 epoch sec; seqNo:1872 2024-11-06T04:48:47Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:47Z- handleDepth()- Depth_Keller msg: 1730868527.29 epoch sec; seqNo:2005; depth 49.96 2024-11-06T04:48:47Z- handleDVL() - DVL msg epochMillisec: 1730868527.29 2024-11-06T04:48:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:47Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:47Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:47Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:47Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:47Z- handleDvl() - RDI_Pathfinder msg: 1730868527.29 sec; seqNo:738; #beams:0 2024-11-06T04:48:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.04 2024-11-06T04:48:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868527.31 epoch sec; seqNo:2005; 4061702.76 north; 594601.30 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:48:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868523.66 2024-11-06T04:48:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:47Z- waiting for 1730868527.66, time is 1730868527.31 2024-11-06T04:48:47Z- AHRS_M2 msg: 1730868527.69 epoch sec; seqNo:1873 2024-11-06T04:48:47Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:47Z- handleDepth()- Depth_Keller msg: 1730868527.69 epoch sec; seqNo:2006; depth 49.96 2024-11-06T04:48:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868527.70 epoch sec; seqNo:2006; 4061702.76 north; 594601.30 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:48:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868523.66 2024-11-06T04:48:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:47Z- waiting for 1730868527.66, time is 1730868527.70 2024-11-06T04:48:47Z- LcmTrn::updateTrn() >>>> pose time 1730868527.70, meas time 1730868527.29 <<<< 2024-11-06T04:48:47Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868527.70 2024-11-06T04:48:47Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:48:47Z- TerrainNav::motionUpdate() - currEstimate = (4061703, 594601.30, 49.96) 2024-11-06T04:48:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:48:47Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:48:47Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:48:47Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:48:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:48:47Z- variances([0], [2], and psi_berg) = 0.419402 0.542251 0.000000 2024-11-06T04:48:47Z- publishEstimatesDecoder() - at 1730868527.70 sec 2024-11-06T04:48:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:48:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061646.51 594604.76 55.07 2024-11-06T04:48:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061646.48 594605.74 53.94 lat/lon: 36.70 -121.94 2024-11-06T04:48:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.42 0.54 1.09 0.00 2024-11-06T04:48:48Z- AHRS_M2 msg: 1730868528.11 epoch sec; seqNo:1874 2024-11-06T04:48:48Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:48Z- handleDepth()- Depth_Keller msg: 1730868528.13 epoch sec; seqNo:2007; depth 49.98 2024-11-06T04:48:48Z- handleDVL() - DVL msg epochMillisec: 1730868528.17 2024-11-06T04:48:48Z- handleDvl() - RDI_Pathfinder msg: 1730868528.17 sec; seqNo:739; #beams:4 2024-11-06T04:48:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.59 , 33.31 , 34.49 , 37.38 2024-11-06T04:48:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.06 , 0.01 2024-11-06T04:48:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868528.19 epoch sec; seqNo:2007; 4061702.76 north; 594601.30 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T04:48:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868527.70 2024-11-06T04:48:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:48Z- waiting for 1730868531.70, time is 1730868528.19 2024-11-06T04:48:48Z- AHRS_M2 msg: 1730868528.50 epoch sec; seqNo:1875 2024-11-06T04:48:48Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:48Z- handleDepth()- Depth_Keller msg: 1730868528.51 epoch sec; seqNo:2008; depth 49.98 2024-11-06T04:48:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868528.52 epoch sec; seqNo:2008; 4061702.77 north; 594602.52 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T04:48:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868527.70 2024-11-06T04:48:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:48Z- waiting for 1730868531.70, time is 1730868528.52 2024-11-06T04:48:48Z- AHRS_M2 msg: 1730868528.90 epoch sec; seqNo:1876 2024-11-06T04:48:48Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:48Z- handleDepth()- Depth_Keller msg: 1730868528.91 epoch sec; seqNo:2009; depth 49.99 2024-11-06T04:48:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868528.92 epoch sec; seqNo:2009; 4061702.77 north; 594602.52 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T04:48:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868527.70 2024-11-06T04:48:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:49Z- waiting for 1730868531.70, time is 1730868528.92 2024-11-06T04:48:49Z- AHRS_M2 msg: 1730868529.30 epoch sec; seqNo:1877 2024-11-06T04:48:49Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:49Z- handleDepth()- Depth_Keller msg: 1730868529.31 epoch sec; seqNo:2010; depth 49.99 2024-11-06T04:48:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868529.32 epoch sec; seqNo:2010; 4061702.77 north; 594602.52 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T04:48:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868527.70 2024-11-06T04:48:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:49Z- waiting for 1730868531.70, time is 1730868529.32 2024-11-06T04:48:49Z- AHRS_M2 msg: 1730868529.71 epoch sec; seqNo:1878 2024-11-06T04:48:49Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:49Z- handleDepth()- Depth_Keller msg: 1730868529.73 epoch sec; seqNo:2011; depth 49.99 2024-11-06T04:48:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868529.74 epoch sec; seqNo:2011; 4061702.79 north; 594603.74 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T04:48:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868527.70 2024-11-06T04:48:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:49Z- waiting for 1730868531.70, time is 1730868529.74 2024-11-06T04:48:50Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:48:50Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:48:50Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:48:50Z- AHRS_M2 msg: 1730868530.12 epoch sec; seqNo:1879 2024-11-06T04:48:50Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:50Z- handleDepth()- Depth_Keller msg: 1730868530.18 epoch sec; seqNo:2012; depth 49.99 2024-11-06T04:48:50Z- handleDVL() - DVL msg epochMillisec: 1730868530.20 2024-11-06T04:48:50Z- handleDvl() - RDI_Pathfinder msg: 1730868530.20 sec; seqNo:740; #beams:4 2024-11-06T04:48:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.42 , 33.31 , 34.32 , 37.21 2024-11-06T04:48:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.03 2024-11-06T04:48:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868530.21 epoch sec; seqNo:2012; 4061702.79 north; 594603.74 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T04:48:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868527.70 2024-11-06T04:48:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:50Z- waiting for 1730868531.70, time is 1730868530.21 2024-11-06T04:48:50Z- AHRS_M2 msg: 1730868530.51 epoch sec; seqNo:1880 2024-11-06T04:48:50Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.54 psi 2024-11-06T04:48:50Z- handleDepth()- Depth_Keller msg: 1730868530.53 epoch sec; seqNo:2013; depth 50.01 2024-11-06T04:48:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868530.54 epoch sec; seqNo:2013; 4061703.17 north; 594603.74 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:48:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868527.70 2024-11-06T04:48:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:50Z- waiting for 1730868531.70, time is 1730868530.54 2024-11-06T04:48:50Z- AHRS_M2 msg: 1730868530.92 epoch sec; seqNo:1881 2024-11-06T04:48:50Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.55 psi 2024-11-06T04:48:50Z- handleDepth()- Depth_Keller msg: 1730868530.93 epoch sec; seqNo:2014; depth 49.99 2024-11-06T04:48:50Z- handleDVL() - DVL msg epochMillisec: 1730868530.93 2024-11-06T04:48:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:50Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:50Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:50Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:50Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:50Z- handleDvl() - RDI_Pathfinder msg: 1730868530.93 sec; seqNo:741; #beams:0 2024-11-06T04:48:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.06 , -0.03 2024-11-06T04:48:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868530.94 epoch sec; seqNo:2014; 4061703.17 north; 594603.74 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T04:48:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868527.70 2024-11-06T04:48:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:51Z- waiting for 1730868531.70, time is 1730868530.94 2024-11-06T04:48:51Z- AHRS_M2 msg: 1730868531.33 epoch sec; seqNo:1882 2024-11-06T04:48:51Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.55 psi 2024-11-06T04:48:51Z- handleDepth()- Depth_Keller msg: 1730868531.34 epoch sec; seqNo:2015; depth 50.00 2024-11-06T04:48:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868531.35 epoch sec; seqNo:2015; 4061703.18 north; 594604.96 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T04:48:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868527.70 2024-11-06T04:48:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:51Z- waiting for 1730868531.70, time is 1730868531.35 2024-11-06T04:48:51Z- AHRS_M2 msg: 1730868531.73 epoch sec; seqNo:1883 2024-11-06T04:48:51Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.55 psi 2024-11-06T04:48:51Z- handleDepth()- Depth_Keller msg: 1730868531.73 epoch sec; seqNo:2016; depth 50.02 2024-11-06T04:48:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868531.76 epoch sec; seqNo:2016; 4061703.18 north; 594604.96 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:48:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868527.70 2024-11-06T04:48:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:51Z- waiting for 1730868531.70, time is 1730868531.76 2024-11-06T04:48:51Z- LcmTrn::updateTrn() >>>> pose time 1730868531.76, meas time 1730868530.93 <<<< 2024-11-06T04:48:51Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868531.76 2024-11-06T04:48:51Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:48:51Z- TerrainNav::motionUpdate() - currEstimate = (4061703, 594604.96, 50.02) 2024-11-06T04:48:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:48:51Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:48:51Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:48:51Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:48:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:48:51Z- variances([0], [2], and psi_berg) = 0.421719 0.543679 0.000000 2024-11-06T04:48:51Z- publishEstimatesDecoder() - at 1730868531.76 sec 2024-11-06T04:48:51Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:48:51Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061646.96 594608.40 54.87 2024-11-06T04:48:51Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061646.90 594609.40 54.00 lat/lon: 36.70 -121.94 2024-11-06T04:48:51Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.42 0.54 1.34 0.00 2024-11-06T04:48:52Z- AHRS_M2 msg: 1730868532.15 epoch sec; seqNo:1884 2024-11-06T04:48:52Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.55 psi 2024-11-06T04:48:52Z- handleDepth()- Depth_Keller msg: 1730868532.16 epoch sec; seqNo:2017; depth 50.01 2024-11-06T04:48:52Z- handleDVL() - DVL msg epochMillisec: 1730868532.19 2024-11-06T04:48:52Z- handleDvl() - RDI_Pathfinder msg: 1730868532.19 sec; seqNo:742; #beams:4 2024-11-06T04:48:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.42 , 32.97 , 34.32 , 37.04 2024-11-06T04:48:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , -0.05 2024-11-06T04:48:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868532.20 epoch sec; seqNo:2017; 4061703.18 north; 594604.96 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:48:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868531.76 2024-11-06T04:48:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:52Z- waiting for 1730868535.76, time is 1730868532.20 2024-11-06T04:48:52Z- AHRS_M2 msg: 1730868532.54 epoch sec; seqNo:1885 2024-11-06T04:48:52Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.55 psi 2024-11-06T04:48:52Z- handleDepth()- Depth_Keller msg: 1730868532.55 epoch sec; seqNo:2018; depth 50.01 2024-11-06T04:48:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868532.57 epoch sec; seqNo:2018; 4061703.19 north; 594606.18 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:48:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868531.76 2024-11-06T04:48:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:52Z- waiting for 1730868535.76, time is 1730868532.57 2024-11-06T04:48:52Z- AHRS_M2 msg: 1730868532.94 epoch sec; seqNo:1886 2024-11-06T04:48:52Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.55 psi 2024-11-06T04:48:52Z- handleDepth()- Depth_Keller msg: 1730868532.95 epoch sec; seqNo:2019; depth 50.03 2024-11-06T04:48:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868532.97 epoch sec; seqNo:2019; 4061703.19 north; 594606.18 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T04:48:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868531.76 2024-11-06T04:48:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:53Z- waiting for 1730868535.76, time is 1730868532.97 2024-11-06T04:48:53Z- AHRS_M2 msg: 1730868533.34 epoch sec; seqNo:1887 2024-11-06T04:48:53Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.55 psi 2024-11-06T04:48:53Z- handleDepth()- Depth_Keller msg: 1730868533.35 epoch sec; seqNo:2020; depth 50.03 2024-11-06T04:48:53Z- handleDVL() - DVL msg epochMillisec: 1730868533.36 2024-11-06T04:48:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:53Z- handleDvl() - RDI_Pathfinder msg: 1730868533.36 sec; seqNo:743; #beams:0 2024-11-06T04:48:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , -0.05 2024-11-06T04:48:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868533.37 epoch sec; seqNo:2020; 4061703.19 north; 594606.18 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T04:48:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868531.76 2024-11-06T04:48:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:53Z- waiting for 1730868535.76, time is 1730868533.37 2024-11-06T04:48:53Z- AHRS_M2 msg: 1730868533.75 epoch sec; seqNo:1888 2024-11-06T04:48:53Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.55 psi 2024-11-06T04:48:53Z- handleDepth()- Depth_Keller msg: 1730868533.76 epoch sec; seqNo:2021; depth 50.04 2024-11-06T04:48:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868533.77 epoch sec; seqNo:2021; 4061703.19 north; 594606.18 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T04:48:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868531.76 2024-11-06T04:48:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:53Z- waiting for 1730868535.76, time is 1730868533.77 2024-11-06T04:48:54Z- AHRS_M2 msg: 1730868534.16 epoch sec; seqNo:1889 2024-11-06T04:48:54Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.55 psi 2024-11-06T04:48:54Z- handleDepth()- Depth_Keller msg: 1730868534.18 epoch sec; seqNo:2022; depth 50.05 2024-11-06T04:48:54Z- handleDVL() - DVL msg epochMillisec: 1730868534.20 2024-11-06T04:48:54Z- handleDvl() - RDI_Pathfinder msg: 1730868534.20 sec; seqNo:744; #beams:4 2024-11-06T04:48:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.08 , 32.97 , 33.99 , 37.04 2024-11-06T04:48:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.03 , -0.00 2024-11-06T04:48:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868534.21 epoch sec; seqNo:2022; 4061703.21 north; 594607.40 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T04:48:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868531.76 2024-11-06T04:48:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:54Z- waiting for 1730868535.76, time is 1730868534.21 2024-11-06T04:48:54Z- AHRS_M2 msg: 1730868534.56 epoch sec; seqNo:1890 2024-11-06T04:48:54Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.55 psi 2024-11-06T04:48:54Z- handleDepth()- Depth_Keller msg: 1730868534.56 epoch sec; seqNo:2023; depth 50.05 2024-11-06T04:48:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868534.59 epoch sec; seqNo:2023; 4061703.21 north; 594607.40 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T04:48:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868531.76 2024-11-06T04:48:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:54Z- waiting for 1730868535.76, time is 1730868534.59 2024-11-06T04:48:54Z- AHRS_M2 msg: 1730868534.96 epoch sec; seqNo:1891 2024-11-06T04:48:54Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.55 psi 2024-11-06T04:48:54Z- handleDepth()- Depth_Keller msg: 1730868534.97 epoch sec; seqNo:2024; depth 50.07 2024-11-06T04:48:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868534.98 epoch sec; seqNo:2024; 4061703.21 north; 594607.40 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T04:48:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868531.76 2024-11-06T04:48:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:55Z- waiting for 1730868535.76, time is 1730868534.98 2024-11-06T04:48:55Z- AHRS_M2 msg: 1730868535.37 epoch sec; seqNo:1892 2024-11-06T04:48:55Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.55 psi 2024-11-06T04:48:55Z- handleDepth()- Depth_Keller msg: 1730868535.38 epoch sec; seqNo:2025; depth 50.08 2024-11-06T04:48:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868535.39 epoch sec; seqNo:2025; 4061703.21 north; 594607.40 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T04:48:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868531.76 2024-11-06T04:48:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:55Z- waiting for 1730868535.76, time is 1730868535.39 2024-11-06T04:48:55Z- AHRS_M2 msg: 1730868535.78 epoch sec; seqNo:1893 2024-11-06T04:48:55Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 1.55 psi 2024-11-06T04:48:55Z- handleDepth()- Depth_Keller msg: 1730868535.79 epoch sec; seqNo:2026; depth 50.08 2024-11-06T04:48:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868535.80 epoch sec; seqNo:2026; 4061703.22 north; 594608.62 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T04:48:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868531.76 2024-11-06T04:48:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:55Z- waiting for 1730868535.76, time is 1730868535.80 2024-11-06T04:48:55Z- LcmTrn::updateTrn() >>>> pose time 1730868535.80, meas time 1730868534.20 <<<< 2024-11-06T04:48:55Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868535.80 2024-11-06T04:48:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868534.20 sec, ping # 0; waiting for INS data more recent than 1730868531.76... 2024-11-06T04:48:55Z- TerrainNav::motionUpdate() - currEstimate = (4061703, 594608.62, 50.08) 2024-11-06T04:48:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:48:55Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:48:55Z- TNavPF::Projecting Measurements 2024-11-06T04:48:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:48:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:48:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:48:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:48:55Z- TNavFilter::4 good beams. 2024-11-06T04:48:55Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:48:55Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:48:55Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:48:55Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:48:55Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:48:55Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:48:55Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:48:55Z- TerrainMapOctree: (vehN, vehE ) = (4061703.20, 594607.17). 2024-11-06T04:48:55Z- TerrainMapOctree: Min Distance = 150.31. 2024-11-06T04:48:55Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:48:56Z- Forcing Subcloud Comparison 2024-11-06T04:48:56Z- Weighting particles with subcloud comparison 2024-11-06T04:48:56Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:48:56Z- meanExpectedMeasDiff: -0.142523 delta_rms_squared: 2.590183 alpha: 0.992338 2024-11-06T04:48:56Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:48:56Z- meanExpectedMeasDiff: -0.018304 delta_rms_squared: 1.828556 alpha: 0.989181 2024-11-06T04:48:56Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:48:56Z- meanExpectedMeasDiff: 0.421551 delta_rms_squared: 2.125132 alpha: 0.990677 2024-11-06T04:48:56Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:48:56Z- meanExpectedMeasDiff: 0.048548 delta_rms_squared: 2.421491 alpha: 0.991808 2024-11-06T04:48:56Z- Subcloud NIS: 0.341393 2024-11-06T04:48:56Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:48:56Z- TNavPF:: sumWeights = 0.535126 2024-11-06T04:48:56Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:48:56Z- TNavPF::First Term of Map Covariance Matrix is 0.42 2024-11-06T04:48:56Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.01 2024-11-06T04:48:56Z- TerrainNav::Resampling particle filter... 2024-11-06T04:48:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868534.20 sec. 2024-11-06T04:48:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:48:56Z- variances([0], [2], and psi_berg) = 0.415796 0.537839 0.000000 2024-11-06T04:48:56Z- publishEstimatesDecoder() - at 1730868535.80 sec 2024-11-06T04:48:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:48:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061647.02 594612.03 54.38 2024-11-06T04:48:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061646.95 594613.05 54.01 lat/lon: 36.70 -121.94 2024-11-06T04:48:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.42 0.54 0.71 0.00 2024-11-06T04:48:56Z- handleDepth()- Depth_Keller msg: 1730868536.20 epoch sec; seqNo:2027; depth 50.11 2024-11-06T04:48:56Z- handleDVL() - DVL msg epochMillisec: 1730868536.25 2024-11-06T04:48:56Z- handleDvl() - RDI_Pathfinder msg: 1730868536.25 sec; seqNo:745; #beams:4 2024-11-06T04:48:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.74 , 32.79 , 33.81 , 37.21 2024-11-06T04:48:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , 0.04 2024-11-06T04:48:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868536.26 epoch sec; seqNo:2027; 4061703.22 north; 594608.62 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T04:48:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868535.80 2024-11-06T04:48:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:56Z- waiting for 1730868539.80, time is 1730868536.26 2024-11-06T04:48:56Z- AHRS_M2 msg: 1730868536.58 epoch sec; seqNo:1894 2024-11-06T04:48:56Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.55 psi 2024-11-06T04:48:56Z- handleDepth()- Depth_Keller msg: 1730868536.58 epoch sec; seqNo:2028; depth 50.14 2024-11-06T04:48:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868536.60 epoch sec; seqNo:2028; 4061703.22 north; 594608.62 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:48:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868535.80 2024-11-06T04:48:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:56Z- waiting for 1730868539.80, time is 1730868536.60 2024-11-06T04:48:56Z- AHRS_M2 msg: 1730868536.98 epoch sec; seqNo:1895 2024-11-06T04:48:56Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.55 psi 2024-11-06T04:48:56Z- handleDepth()- Depth_Keller msg: 1730868536.99 epoch sec; seqNo:2029; depth 50.16 2024-11-06T04:48:56Z- handleDVL() - DVL msg epochMillisec: 1730868536.99 2024-11-06T04:48:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:56Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:56Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:56Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:56Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:56Z- handleDvl() - RDI_Pathfinder msg: 1730868536.99 sec; seqNo:746; #beams:0 2024-11-06T04:48:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , 0.04 2024-11-06T04:48:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868537.00 epoch sec; seqNo:2029; 4061703.23 north; 594609.84 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T04:48:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868535.80 2024-11-06T04:48:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:57Z- waiting for 1730868539.80, time is 1730868537.00 2024-11-06T04:48:57Z- AHRS_M2 msg: 1730868537.38 epoch sec; seqNo:1896 2024-11-06T04:48:57Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.55 psi 2024-11-06T04:48:57Z- handleDepth()- Depth_Keller msg: 1730868537.39 epoch sec; seqNo:2030; depth 50.16 2024-11-06T04:48:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868537.41 epoch sec; seqNo:2030; 4061703.23 north; 594609.84 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T04:48:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868535.80 2024-11-06T04:48:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:57Z- waiting for 1730868539.80, time is 1730868537.41 2024-11-06T04:48:57Z- AHRS_M2 msg: 1730868537.79 epoch sec; seqNo:1897 2024-11-06T04:48:57Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.56 psi 2024-11-06T04:48:57Z- handleDepth()- Depth_Keller msg: 1730868537.79 epoch sec; seqNo:2031; depth 50.19 2024-11-06T04:48:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868537.80 epoch sec; seqNo:2031; 4061703.61 north; 594609.83 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:48:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868535.80 2024-11-06T04:48:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:57Z- waiting for 1730868539.80, time is 1730868537.80 2024-11-06T04:48:58Z- handleDepth()- Depth_Keller msg: 1730868538.22 epoch sec; seqNo:2032; depth 50.20 2024-11-06T04:48:58Z- handleDVL() - DVL msg epochMillisec: 1730868538.25 2024-11-06T04:48:58Z- handleDvl() - RDI_Pathfinder msg: 1730868538.25 sec; seqNo:747; #beams:4 2024-11-06T04:48:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.57 , 32.79 , 33.81 , 37.04 2024-11-06T04:48:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.05 2024-11-06T04:48:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868538.26 epoch sec; seqNo:2032; 4061703.61 north; 594609.83 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:48:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868535.80 2024-11-06T04:48:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:58Z- waiting for 1730868539.80, time is 1730868538.26 2024-11-06T04:48:58Z- AHRS_M2 msg: 1730868538.60 epoch sec; seqNo:1898 2024-11-06T04:48:58Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.56 psi 2024-11-06T04:48:58Z- handleDepth()- Depth_Keller msg: 1730868538.61 epoch sec; seqNo:2033; depth 50.23 2024-11-06T04:48:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868538.62 epoch sec; seqNo:2033; 4061703.63 north; 594611.05 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T04:48:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868535.80 2024-11-06T04:48:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:58Z- waiting for 1730868539.80, time is 1730868538.62 2024-11-06T04:48:59Z- AHRS_M2 msg: 1730868539.00 epoch sec; seqNo:1899 2024-11-06T04:48:59Z- AHRS_M2 msg: 0.14 phi; -0.06 theta; 1.56 psi 2024-11-06T04:48:59Z- handleDepth()- Depth_Keller msg: 1730868539.00 epoch sec; seqNo:2034; depth 50.22 2024-11-06T04:48:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868539.02 epoch sec; seqNo:2034; 4061703.63 north; 594611.05 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T04:48:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868535.80 2024-11-06T04:48:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:59Z- waiting for 1730868539.80, time is 1730868539.02 2024-11-06T04:48:59Z- AHRS_M2 msg: 1730868539.40 epoch sec; seqNo:1900 2024-11-06T04:48:59Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.56 psi 2024-11-06T04:48:59Z- handleDepth()- Depth_Keller msg: 1730868539.41 epoch sec; seqNo:2035; depth 50.25 2024-11-06T04:48:59Z- handleDVL() - DVL msg epochMillisec: 1730868539.41 2024-11-06T04:48:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:48:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:48:59Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:48:59Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:48:59Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:48:59Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:48:59Z- handleDvl() - RDI_Pathfinder msg: 1730868539.41 sec; seqNo:748; #beams:0 2024-11-06T04:48:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:48:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.05 2024-11-06T04:48:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868539.42 epoch sec; seqNo:2035; 4061703.63 north; 594611.05 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T04:48:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:48:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868535.80 2024-11-06T04:48:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:59Z- waiting for 1730868539.80, time is 1730868539.42 2024-11-06T04:48:59Z- AHRS_M2 msg: 1730868539.81 epoch sec; seqNo:1901 2024-11-06T04:48:59Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.56 psi 2024-11-06T04:48:59Z- handleDepth()- Depth_Keller msg: 1730868539.82 epoch sec; seqNo:2036; depth 50.27 2024-11-06T04:48:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868539.83 epoch sec; seqNo:2036; 4061703.63 north; 594611.05 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T04:48:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:48:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868535.80 2024-11-06T04:48:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:48:59Z- waiting for 1730868539.80, time is 1730868539.83 2024-11-06T04:48:59Z- LcmTrn::updateTrn() >>>> pose time 1730868539.83, meas time 1730868539.41 <<<< 2024-11-06T04:48:59Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868539.83 2024-11-06T04:48:59Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:48:59Z- TerrainNav::motionUpdate() - currEstimate = (4061704, 594611.05, 50.27) 2024-11-06T04:48:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:48:59Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:48:59Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:48:59Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:48:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:49:00Z- variances([0], [2], and psi_berg) = 0.416011 0.538915 0.000000 2024-11-06T04:49:00Z- publishEstimatesDecoder() - at 1730868539.83 sec 2024-11-06T04:49:00Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:49:00Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061647.46 594614.50 54.96 2024-11-06T04:49:00Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061647.35 594615.49 54.20 lat/lon: 36.70 -121.94 2024-11-06T04:49:00Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.42 0.54 0.96 0.00 2024-11-06T04:49:00Z- handleDepth()- Depth_Keller msg: 1730868540.24 epoch sec; seqNo:2037; depth 50.28 2024-11-06T04:49:00Z- handleDVL() - DVL msg epochMillisec: 1730868540.25 2024-11-06T04:49:00Z- handleDvl() - RDI_Pathfinder msg: 1730868540.25 sec; seqNo:749; #beams:4 2024-11-06T04:49:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.40 , 32.97 , 33.65 , 36.87 2024-11-06T04:49:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , 0.04 2024-11-06T04:49:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868540.27 epoch sec; seqNo:2037; 4061703.64 north; 594612.27 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T04:49:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868539.83 2024-11-06T04:49:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:00Z- waiting for 1730868543.83, time is 1730868540.27 2024-11-06T04:49:00Z- AHRS_M2 msg: 1730868540.62 epoch sec; seqNo:1902 2024-11-06T04:49:00Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.56 psi 2024-11-06T04:49:00Z- handleDepth()- Depth_Keller msg: 1730868540.63 epoch sec; seqNo:2038; depth 50.29 2024-11-06T04:49:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868540.64 epoch sec; seqNo:2038; 4061703.64 north; 594612.27 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:49:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868539.83 2024-11-06T04:49:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:00Z- waiting for 1730868543.83, time is 1730868540.64 2024-11-06T04:49:01Z- AHRS_M2 msg: 1730868541.02 epoch sec; seqNo:1903 2024-11-06T04:49:01Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.56 psi 2024-11-06T04:49:01Z- handleDepth()- Depth_Keller msg: 1730868541.04 epoch sec; seqNo:2039; depth 50.30 2024-11-06T04:49:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868541.08 epoch sec; seqNo:2039; 4061703.64 north; 594612.27 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:49:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868539.83 2024-11-06T04:49:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:01Z- waiting for 1730868543.83, time is 1730868541.08 2024-11-06T04:49:01Z- AHRS_M2 msg: 1730868541.42 epoch sec; seqNo:1904 2024-11-06T04:49:01Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.56 psi 2024-11-06T04:49:01Z- handleDepth()- Depth_Keller msg: 1730868541.43 epoch sec; seqNo:2040; depth 50.31 2024-11-06T04:49:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868541.44 epoch sec; seqNo:2040; 4061703.65 north; 594613.49 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:49:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868539.83 2024-11-06T04:49:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:01Z- waiting for 1730868543.83, time is 1730868541.44 2024-11-06T04:49:01Z- AHRS_M2 msg: 1730868541.83 epoch sec; seqNo:1905 2024-11-06T04:49:01Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.56 psi 2024-11-06T04:49:01Z- handleDepth()- Depth_Keller msg: 1730868541.84 epoch sec; seqNo:2041; depth 50.32 2024-11-06T04:49:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868541.85 epoch sec; seqNo:2041; 4061703.65 north; 594613.49 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:49:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868539.83 2024-11-06T04:49:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:01Z- waiting for 1730868543.83, time is 1730868541.85 2024-11-06T04:49:02Z- AHRS_M2 msg: 1730868542.23 epoch sec; seqNo:1906 2024-11-06T04:49:02Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.56 psi 2024-11-06T04:49:02Z- handleDepth()- Depth_Keller msg: 1730868542.24 epoch sec; seqNo:2042; depth 50.33 2024-11-06T04:49:02Z- handleDVL() - DVL msg epochMillisec: 1730868542.27 2024-11-06T04:49:02Z- handleDvl() - RDI_Pathfinder msg: 1730868542.27 sec; seqNo:750; #beams:4 2024-11-06T04:49:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.23 , 32.79 , 33.65 , 36.70 2024-11-06T04:49:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.07 , -0.00 2024-11-06T04:49:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868542.28 epoch sec; seqNo:2042; 4061703.65 north; 594613.49 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T04:49:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868539.83 2024-11-06T04:49:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:02Z- waiting for 1730868543.83, time is 1730868542.28 2024-11-06T04:49:02Z- AHRS_M2 msg: 1730868542.63 epoch sec; seqNo:1907 2024-11-06T04:49:02Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.56 psi 2024-11-06T04:49:02Z- handleDepth()- Depth_Keller msg: 1730868542.65 epoch sec; seqNo:2043; depth 50.33 2024-11-06T04:49:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868542.67 epoch sec; seqNo:2043; 4061703.67 north; 594614.71 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T04:49:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868539.83 2024-11-06T04:49:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:02Z- waiting for 1730868543.83, time is 1730868542.67 2024-11-06T04:49:03Z- AHRS_M2 msg: 1730868543.04 epoch sec; seqNo:1908 2024-11-06T04:49:03Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.56 psi 2024-11-06T04:49:03Z- handleDepth()- Depth_Keller msg: 1730868543.05 epoch sec; seqNo:2044; depth 50.35 2024-11-06T04:49:03Z- handleDVL() - DVL msg epochMillisec: 1730868543.05 2024-11-06T04:49:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:03Z- handleDvl() - RDI_Pathfinder msg: 1730868543.05 sec; seqNo:751; #beams:0 2024-11-06T04:49:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.07 , -0.00 2024-11-06T04:49:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868543.06 epoch sec; seqNo:2044; 4061703.67 north; 594614.71 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:49:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868539.83 2024-11-06T04:49:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:03Z- waiting for 1730868543.83, time is 1730868543.06 2024-11-06T04:49:03Z- AHRS_M2 msg: 1730868543.44 epoch sec; seqNo:1909 2024-11-06T04:49:03Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.56 psi 2024-11-06T04:49:03Z- handleDepth()- Depth_Keller msg: 1730868543.45 epoch sec; seqNo:2045; depth 50.34 2024-11-06T04:49:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868543.46 epoch sec; seqNo:2045; 4061703.67 north; 594614.71 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:49:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868539.83 2024-11-06T04:49:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:03Z- waiting for 1730868543.83, time is 1730868543.46 2024-11-06T04:49:03Z- AHRS_M2 msg: 1730868543.85 epoch sec; seqNo:1910 2024-11-06T04:49:03Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.57 psi 2024-11-06T04:49:03Z- handleDepth()- Depth_Keller msg: 1730868543.86 epoch sec; seqNo:2046; depth 50.35 2024-11-06T04:49:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868543.88 epoch sec; seqNo:2046; 4061703.68 north; 594615.93 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:49:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868539.83 2024-11-06T04:49:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:04Z- waiting for 1730868543.83, time is 1730868543.88 2024-11-06T04:49:04Z- LcmTrn::updateTrn() >>>> pose time 1730868543.88, meas time 1730868543.05 <<<< 2024-11-06T04:49:04Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868543.88 2024-11-06T04:49:04Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:49:04Z- TerrainNav::motionUpdate() - currEstimate = (4061704, 594615.93, 50.35) 2024-11-06T04:49:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:49:04Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:49:04Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:49:04Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:49:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:49:04Z- variances([0], [2], and psi_berg) = 0.420688 0.542997 0.000000 2024-11-06T04:49:04Z- publishEstimatesDecoder() - at 1730868543.88 sec 2024-11-06T04:49:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:49:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061647.46 594619.37 55.48 2024-11-06T04:49:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061647.41 594620.37 54.28 lat/lon: 36.70 -121.94 2024-11-06T04:49:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.42 0.54 1.22 0.00 2024-11-06T04:49:04Z- AHRS_M2 msg: 1730868544.25 epoch sec; seqNo:1911 2024-11-06T04:49:04Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.57 psi 2024-11-06T04:49:04Z- handleDepth()- Depth_Keller msg: 1730868544.26 epoch sec; seqNo:2047; depth 50.35 2024-11-06T04:49:04Z- handleDVL() - DVL msg epochMillisec: 1730868544.28 2024-11-06T04:49:04Z- handleDvl() - RDI_Pathfinder msg: 1730868544.28 sec; seqNo:752; #beams:4 2024-11-06T04:49:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.06 , 32.46 , 33.65 , 36.53 2024-11-06T04:49:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , -0.04 2024-11-06T04:49:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868544.30 epoch sec; seqNo:2047; 4061704.06 north; 594615.93 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:49:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868543.88 2024-11-06T04:49:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:04Z- waiting for 1730868547.88, time is 1730868544.30 2024-11-06T04:49:04Z- AHRS_M2 msg: 1730868544.66 epoch sec; seqNo:1912 2024-11-06T04:49:04Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.57 psi 2024-11-06T04:49:04Z- handleDepth()- Depth_Keller msg: 1730868544.66 epoch sec; seqNo:2048; depth 50.35 2024-11-06T04:49:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868544.69 epoch sec; seqNo:2048; 4061704.06 north; 594615.93 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:49:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868543.88 2024-11-06T04:49:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:04Z- waiting for 1730868547.88, time is 1730868544.69 2024-11-06T04:49:05Z- AHRS_M2 msg: 1730868545.06 epoch sec; seqNo:1913 2024-11-06T04:49:05Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.57 psi 2024-11-06T04:49:05Z- handleDepth()- Depth_Keller msg: 1730868545.06 epoch sec; seqNo:2049; depth 50.34 2024-11-06T04:49:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868545.07 epoch sec; seqNo:2049; 4061704.06 north; 594615.93 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:49:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868543.88 2024-11-06T04:49:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:05Z- waiting for 1730868547.88, time is 1730868545.07 2024-11-06T04:49:05Z- AHRS_M2 msg: 1730868545.47 epoch sec; seqNo:1914 2024-11-06T04:49:05Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.57 psi 2024-11-06T04:49:05Z- handleDepth()- Depth_Keller msg: 1730868545.47 epoch sec; seqNo:2050; depth 50.36 2024-11-06T04:49:05Z- handleDVL() - DVL msg epochMillisec: 1730868545.47 2024-11-06T04:49:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:05Z- handleDvl() - RDI_Pathfinder msg: 1730868545.47 sec; seqNo:753; #beams:0 2024-11-06T04:49:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , -0.04 2024-11-06T04:49:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868545.48 epoch sec; seqNo:2050; 4061704.07 north; 594617.15 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:49:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868543.88 2024-11-06T04:49:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:05Z- waiting for 1730868547.88, time is 1730868545.48 2024-11-06T04:49:05Z- AHRS_M2 msg: 1730868545.87 epoch sec; seqNo:1915 2024-11-06T04:49:05Z- AHRS_M2 msg: 0.14 phi; -0.05 theta; 1.56 psi 2024-11-06T04:49:05Z- handleDepth()- Depth_Keller msg: 1730868545.88 epoch sec; seqNo:2051; depth 50.36 2024-11-06T04:49:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868545.91 epoch sec; seqNo:2051; 4061704.07 north; 594617.15 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:49:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868543.88 2024-11-06T04:49:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:06Z- waiting for 1730868547.88, time is 1730868545.91 2024-11-06T04:49:06Z- AHRS_M2 msg: 1730868546.27 epoch sec; seqNo:1916 2024-11-06T04:49:06Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.56 psi 2024-11-06T04:49:06Z- handleDepth()- Depth_Keller msg: 1730868546.29 epoch sec; seqNo:2052; depth 50.37 2024-11-06T04:49:06Z- handleDVL() - DVL msg epochMillisec: 1730868546.31 2024-11-06T04:49:06Z- handleDvl() - RDI_Pathfinder msg: 1730868546.31 sec; seqNo:754; #beams:4 2024-11-06T04:49:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.06 , 32.28 , 33.65 , 36.02 2024-11-06T04:49:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , -0.05 2024-11-06T04:49:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868546.33 epoch sec; seqNo:2052; 4061704.07 north; 594617.15 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:49:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868543.88 2024-11-06T04:49:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:06Z- waiting for 1730868547.88, time is 1730868546.33 2024-11-06T04:49:06Z- AHRS_M2 msg: 1730868546.68 epoch sec; seqNo:1917 2024-11-06T04:49:06Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 1.56 psi 2024-11-06T04:49:06Z- handleDepth()- Depth_Keller msg: 1730868546.69 epoch sec; seqNo:2053; depth 50.37 2024-11-06T04:49:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868546.70 epoch sec; seqNo:2053; 4061704.09 north; 594618.37 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:49:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868543.88 2024-11-06T04:49:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:06Z- waiting for 1730868547.88, time is 1730868546.70 2024-11-06T04:49:07Z- AHRS_M2 msg: 1730868547.08 epoch sec; seqNo:1918 2024-11-06T04:49:07Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 1.56 psi 2024-11-06T04:49:07Z- handleDepth()- Depth_Keller msg: 1730868547.09 epoch sec; seqNo:2054; depth 50.37 2024-11-06T04:49:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868547.10 epoch sec; seqNo:2054; 4061704.09 north; 594618.37 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:49:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868543.88 2024-11-06T04:49:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:07Z- waiting for 1730868547.88, time is 1730868547.10 2024-11-06T04:49:07Z- AHRS_M2 msg: 1730868547.48 epoch sec; seqNo:1919 2024-11-06T04:49:07Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 1.56 psi 2024-11-06T04:49:07Z- handleDepth()- Depth_Keller msg: 1730868547.50 epoch sec; seqNo:2055; depth 50.38 2024-11-06T04:49:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868547.51 epoch sec; seqNo:2055; 4061704.09 north; 594618.37 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:49:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868543.88 2024-11-06T04:49:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:07Z- waiting for 1730868547.88, time is 1730868547.51 2024-11-06T04:49:07Z- AHRS_M2 msg: 1730868547.89 epoch sec; seqNo:1920 2024-11-06T04:49:07Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 1.56 psi 2024-11-06T04:49:07Z- handleDepth()- Depth_Keller msg: 1730868547.90 epoch sec; seqNo:2056; depth 50.39 2024-11-06T04:49:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868547.91 epoch sec; seqNo:2056; 4061704.09 north; 594618.37 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:49:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868543.88 2024-11-06T04:49:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:08Z- waiting for 1730868547.88, time is 1730868547.91 2024-11-06T04:49:08Z- LcmTrn::updateTrn() >>>> pose time 1730868547.91, meas time 1730868546.31 <<<< 2024-11-06T04:49:08Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868547.91 2024-11-06T04:49:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868546.31 sec, ping # 0; waiting for INS data more recent than 1730868543.88... 2024-11-06T04:49:08Z- TerrainNav::motionUpdate() - currEstimate = (4061704, 594618.37, 50.39) 2024-11-06T04:49:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:49:08Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:49:08Z- TNavPF::Projecting Measurements 2024-11-06T04:49:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:49:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:49:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:49:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:49:08Z- TNavFilter::4 good beams. 2024-11-06T04:49:08Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:49:08Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:49:08Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:49:08Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:49:08Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:49:08Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:49:08Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:49:08Z- TerrainMapOctree: (vehN, vehE ) = (4061703.93, 594617.41). 2024-11-06T04:49:08Z- TerrainMapOctree: Min Distance = 140.37. 2024-11-06T04:49:08Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:49:08Z- Forcing Subcloud Comparison 2024-11-06T04:49:08Z- Weighting particles with subcloud comparison 2024-11-06T04:49:08Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:08Z- meanExpectedMeasDiff: -0.364022 delta_rms_squared: 2.317739 alpha: 0.991445 2024-11-06T04:49:08Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:08Z- meanExpectedMeasDiff: -0.577433 delta_rms_squared: 1.706392 alpha: 0.988415 2024-11-06T04:49:08Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:08Z- meanExpectedMeasDiff: 0.239453 delta_rms_squared: 1.881872 alpha: 0.989484 2024-11-06T04:49:08Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:08Z- meanExpectedMeasDiff: -0.335752 delta_rms_squared: 2.074024 alpha: 0.990449 2024-11-06T04:49:08Z- Subcloud NIS: 0.322284 2024-11-06T04:49:08Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:49:08Z- TNavPF:: sumWeights = 0.530974 2024-11-06T04:49:08Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:49:08Z- TNavPF::First Term of Map Covariance Matrix is 0.40 2024-11-06T04:49:08Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.01 2024-11-06T04:49:08Z- TerrainNav::Resampling particle filter... 2024-11-06T04:49:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868546.31 sec. 2024-11-06T04:49:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:49:08Z- variances([0], [2], and psi_berg) = 0.419487 0.538540 0.000000 2024-11-06T04:49:08Z- publishEstimatesDecoder() - at 1730868547.91 sec 2024-11-06T04:49:08Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:49:08Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061647.86 594621.87 56.11 2024-11-06T04:49:08Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061647.81 594622.80 54.48 lat/lon: 36.70 -121.94 2024-11-06T04:49:08Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.42 0.54 0.73 0.00 2024-11-06T04:49:08Z- AHRS_M2 msg: 1730868548.30 epoch sec; seqNo:1921 2024-11-06T04:49:08Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 1.56 psi 2024-11-06T04:49:08Z- handleDepth()- Depth_Keller msg: 1730868548.30 epoch sec; seqNo:2057; depth 50.40 2024-11-06T04:49:08Z- handleDVL() - DVL msg epochMillisec: 1730868548.34 2024-11-06T04:49:08Z- handleDvl() - RDI_Pathfinder msg: 1730868548.34 sec; seqNo:755; #beams:4 2024-11-06T04:49:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.38 , 32.28 , 33.65 , 35.85 2024-11-06T04:49:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.05 , -0.01 2024-11-06T04:49:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868548.36 epoch sec; seqNo:2057; 4061704.10 north; 594619.59 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T04:49:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868547.91 2024-11-06T04:49:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:08Z- waiting for 1730868551.91, time is 1730868548.36 2024-11-06T04:49:08Z- AHRS_M2 msg: 1730868548.70 epoch sec; seqNo:1922 2024-11-06T04:49:08Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 1.56 psi 2024-11-06T04:49:08Z- handleDepth()- Depth_Keller msg: 1730868548.70 epoch sec; seqNo:2058; depth 50.42 2024-11-06T04:49:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868548.72 epoch sec; seqNo:2058; 4061704.10 north; 594619.59 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T04:49:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868547.91 2024-11-06T04:49:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:08Z- waiting for 1730868551.91, time is 1730868548.72 2024-11-06T04:49:09Z- AHRS_M2 msg: 1730868549.10 epoch sec; seqNo:1923 2024-11-06T04:49:09Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 1.55 psi 2024-11-06T04:49:09Z- handleDepth()- Depth_Keller msg: 1730868549.12 epoch sec; seqNo:2059; depth 50.42 2024-11-06T04:49:09Z- handleDVL() - DVL msg epochMillisec: 1730868549.12 2024-11-06T04:49:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:09Z- handleDvl() - RDI_Pathfinder msg: 1730868549.12 sec; seqNo:756; #beams:0 2024-11-06T04:49:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.05 , -0.01 2024-11-06T04:49:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868549.13 epoch sec; seqNo:2059; 4061704.10 north; 594619.59 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T04:49:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868547.91 2024-11-06T04:49:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:09Z- waiting for 1730868551.91, time is 1730868549.13 2024-11-06T04:49:09Z- AHRS_M2 msg: 1730868549.50 epoch sec; seqNo:1924 2024-11-06T04:49:09Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 1.55 psi 2024-11-06T04:49:09Z- handleDepth()- Depth_Keller msg: 1730868549.51 epoch sec; seqNo:2060; depth 50.44 2024-11-06T04:49:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868549.52 epoch sec; seqNo:2060; 4061704.10 north; 594619.59 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T04:49:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868547.91 2024-11-06T04:49:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:09Z- waiting for 1730868551.91, time is 1730868549.52 2024-11-06T04:49:09Z- handleDepth()- Depth_Keller msg: 1730868549.93 epoch sec; seqNo:2061; depth 50.44 2024-11-06T04:49:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868549.96 epoch sec; seqNo:2061; 4061704.11 north; 594620.81 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T04:49:10Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:49:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868547.91 2024-11-06T04:49:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:10Z- waiting for 1730868551.91, time is 1730868549.96 2024-11-06T04:49:10Z- AHRS_M2 msg: 1730868550.31 epoch sec; seqNo:1925 2024-11-06T04:49:10Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 1.55 psi 2024-11-06T04:49:10Z- handleDepth()- Depth_Keller msg: 1730868550.32 epoch sec; seqNo:2062; depth 50.46 2024-11-06T04:49:10Z- handleDVL() - DVL msg epochMillisec: 1730868550.35 2024-11-06T04:49:10Z- handleDvl() - RDI_Pathfinder msg: 1730868550.35 sec; seqNo:757; #beams:4 2024-11-06T04:49:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.55 , 32.12 , 33.31 , 36.02 2024-11-06T04:49:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.04 , 0.05 2024-11-06T04:49:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868550.38 epoch sec; seqNo:2062; 4061704.11 north; 594620.81 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T04:49:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868547.91 2024-11-06T04:49:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:10Z- waiting for 1730868551.91, time is 1730868550.38 2024-11-06T04:49:10Z- AHRS_M2 msg: 1730868550.72 epoch sec; seqNo:1926 2024-11-06T04:49:10Z- AHRS_M2 msg: 0.13 phi; -0.04 theta; 1.55 psi 2024-11-06T04:49:10Z- handleDepth()- Depth_Keller msg: 1730868550.73 epoch sec; seqNo:2063; depth 50.49 2024-11-06T04:49:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868550.79 epoch sec; seqNo:2063; 4061704.11 north; 594620.81 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T04:49:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868547.91 2024-11-06T04:49:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:10Z- waiting for 1730868551.91, time is 1730868550.79 2024-11-06T04:49:11Z- handleDepth()- Depth_Keller msg: 1730868551.15 epoch sec; seqNo:2064; depth 50.49 2024-11-06T04:49:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868551.16 epoch sec; seqNo:2064; 4061704.11 north; 594620.81 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T04:49:11Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:49:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868547.91 2024-11-06T04:49:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:11Z- waiting for 1730868551.91, time is 1730868551.16 2024-11-06T04:49:11Z- AHRS_M2 msg: 1730868551.52 epoch sec; seqNo:1927 2024-11-06T04:49:11Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 1.54 psi 2024-11-06T04:49:11Z- handleDepth()- Depth_Keller msg: 1730868551.53 epoch sec; seqNo:2065; depth 50.51 2024-11-06T04:49:11Z- handleDVL() - DVL msg epochMillisec: 1730868551.54 2024-11-06T04:49:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:11Z- handleDvl() - RDI_Pathfinder msg: 1730868551.54 sec; seqNo:758; #beams:0 2024-11-06T04:49:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.04 , 0.05 2024-11-06T04:49:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868551.55 epoch sec; seqNo:2065; 4061704.51 north; 594622.03 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T04:49:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868547.91 2024-11-06T04:49:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:11Z- waiting for 1730868551.91, time is 1730868551.55 2024-11-06T04:49:11Z- handleDepth()- Depth_Keller msg: 1730868551.95 epoch sec; seqNo:2066; depth 50.52 2024-11-06T04:49:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868551.97 epoch sec; seqNo:2066; 4061704.51 north; 594622.03 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T04:49:12Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:49:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868547.91 2024-11-06T04:49:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:12Z- waiting for 1730868551.91, time is 1730868551.97 2024-11-06T04:49:12Z- LcmTrn::updateTrn() >>>> pose time 1730868551.97, meas time 1730868551.54 <<<< 2024-11-06T04:49:12Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868551.97 2024-11-06T04:49:12Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:49:12Z- TerrainNav::motionUpdate() - currEstimate = (4061705, 594622.03, 50.52) 2024-11-06T04:49:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:49:12Z- TerrainNav:: SubcloudNIS is 0.32, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:49:12Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:49:12Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:49:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:49:12Z- variances([0], [2], and psi_berg) = 0.421589 0.539784 0.000000 2024-11-06T04:49:12Z- publishEstimatesDecoder() - at 1730868551.97 sec 2024-11-06T04:49:12Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:49:12Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061648.27 594625.46 56.18 2024-11-06T04:49:12Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061648.23 594626.46 54.61 lat/lon: 36.70 -121.94 2024-11-06T04:49:12Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.42 0.54 0.98 0.00 2024-11-06T04:49:12Z- AHRS_M2 msg: 1730868552.34 epoch sec; seqNo:1928 2024-11-06T04:49:12Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.54 psi 2024-11-06T04:49:12Z- handleDepth()- Depth_Keller msg: 1730868552.36 epoch sec; seqNo:2067; depth 50.54 2024-11-06T04:49:12Z- handleDVL() - DVL msg epochMillisec: 1730868552.38 2024-11-06T04:49:12Z- handleDvl() - RDI_Pathfinder msg: 1730868552.38 sec; seqNo:759; #beams:4 2024-11-06T04:49:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.55 , 32.46 , 33.47 , 36.02 2024-11-06T04:49:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , 0.07 2024-11-06T04:49:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868552.39 epoch sec; seqNo:2067; 4061704.51 north; 594622.03 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T04:49:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868551.97 2024-11-06T04:49:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:12Z- waiting for 1730868555.97, time is 1730868552.39 2024-11-06T04:49:12Z- AHRS_M2 msg: 1730868552.74 epoch sec; seqNo:1929 2024-11-06T04:49:12Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.54 psi 2024-11-06T04:49:12Z- handleDepth()- Depth_Keller msg: 1730868552.74 epoch sec; seqNo:2068; depth 50.55 2024-11-06T04:49:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868552.76 epoch sec; seqNo:2068; 4061704.52 north; 594623.25 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:49:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868551.97 2024-11-06T04:49:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:12Z- waiting for 1730868555.97, time is 1730868552.76 2024-11-06T04:49:13Z- AHRS_M2 msg: 1730868553.14 epoch sec; seqNo:1930 2024-11-06T04:49:13Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.54 psi 2024-11-06T04:49:13Z- handleDepth()- Depth_Keller msg: 1730868553.15 epoch sec; seqNo:2069; depth 50.57 2024-11-06T04:49:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868553.16 epoch sec; seqNo:2069; 4061704.52 north; 594623.25 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T04:49:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868551.97 2024-11-06T04:49:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:13Z- waiting for 1730868555.97, time is 1730868553.16 2024-11-06T04:49:13Z- AHRS_M2 msg: 1730868553.54 epoch sec; seqNo:1931 2024-11-06T04:49:13Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.54 psi 2024-11-06T04:49:13Z- handleDepth()- Depth_Keller msg: 1730868553.56 epoch sec; seqNo:2070; depth 50.57 2024-11-06T04:49:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868553.57 epoch sec; seqNo:2070; 4061704.52 north; 594623.25 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T04:49:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868551.97 2024-11-06T04:49:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:13Z- waiting for 1730868555.97, time is 1730868553.57 2024-11-06T04:49:13Z- AHRS_M2 msg: 1730868553.95 epoch sec; seqNo:1932 2024-11-06T04:49:13Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.54 psi 2024-11-06T04:49:13Z- handleDepth()- Depth_Keller msg: 1730868553.96 epoch sec; seqNo:2071; depth 50.59 2024-11-06T04:49:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868553.99 epoch sec; seqNo:2071; 4061704.52 north; 594623.25 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T04:49:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868551.97 2024-11-06T04:49:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:14Z- waiting for 1730868555.97, time is 1730868553.99 2024-11-06T04:49:14Z- AHRS_M2 msg: 1730868554.35 epoch sec; seqNo:1933 2024-11-06T04:49:14Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.55 psi 2024-11-06T04:49:14Z- handleDepth()- Depth_Keller msg: 1730868554.35 epoch sec; seqNo:2072; depth 50.61 2024-11-06T04:49:14Z- handleDVL() - DVL msg epochMillisec: 1730868554.37 2024-11-06T04:49:14Z- handleDvl() - RDI_Pathfinder msg: 1730868554.37 sec; seqNo:760; #beams:4 2024-11-06T04:49:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.55 , 32.12 , 33.47 , 35.85 2024-11-06T04:49:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , 0.03 2024-11-06T04:49:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868554.41 epoch sec; seqNo:2072; 4061704.53 north; 594624.47 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T04:49:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868551.97 2024-11-06T04:49:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:14Z- waiting for 1730868555.97, time is 1730868554.41 2024-11-06T04:49:14Z- AHRS_M2 msg: 1730868554.76 epoch sec; seqNo:1934 2024-11-06T04:49:14Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.55 psi 2024-11-06T04:49:14Z- handleDepth()- Depth_Keller msg: 1730868554.77 epoch sec; seqNo:2073; depth 50.61 2024-11-06T04:49:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868554.79 epoch sec; seqNo:2073; 4061704.53 north; 594624.47 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T04:49:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868551.97 2024-11-06T04:49:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:14Z- waiting for 1730868555.97, time is 1730868554.79 2024-11-06T04:49:15Z- AHRS_M2 msg: 1730868555.16 epoch sec; seqNo:1935 2024-11-06T04:49:15Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.55 psi 2024-11-06T04:49:15Z- handleDepth()- Depth_Keller msg: 1730868555.16 epoch sec; seqNo:2074; depth 50.61 2024-11-06T04:49:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868555.17 epoch sec; seqNo:2074; 4061704.53 north; 594624.47 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T04:49:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868551.97 2024-11-06T04:49:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:15Z- waiting for 1730868555.97, time is 1730868555.17 2024-11-06T04:49:15Z- AHRS_M2 msg: 1730868555.56 epoch sec; seqNo:1936 2024-11-06T04:49:15Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.55 psi 2024-11-06T04:49:15Z- handleDepth()- Depth_Keller msg: 1730868555.57 epoch sec; seqNo:2075; depth 50.62 2024-11-06T04:49:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868555.59 epoch sec; seqNo:2075; 4061704.55 north; 594625.69 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T04:49:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868551.97 2024-11-06T04:49:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:15Z- waiting for 1730868555.97, time is 1730868555.59 2024-11-06T04:49:15Z- AHRS_M2 msg: 1730868555.97 epoch sec; seqNo:1937 2024-11-06T04:49:15Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.55 psi 2024-11-06T04:49:15Z- handleDepth()- Depth_Keller msg: 1730868555.97 epoch sec; seqNo:2076; depth 50.62 2024-11-06T04:49:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868555.98 epoch sec; seqNo:2076; 4061704.55 north; 594625.69 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T04:49:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868551.97 2024-11-06T04:49:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:16Z- waiting for 1730868555.97, time is 1730868555.98 2024-11-06T04:49:16Z- LcmTrn::updateTrn() >>>> pose time 1730868555.98, meas time 1730868554.37 <<<< 2024-11-06T04:49:16Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868555.98 2024-11-06T04:49:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868554.37 sec, ping # 0; waiting for INS data more recent than 1730868551.97... 2024-11-06T04:49:16Z- TerrainNav::motionUpdate() - currEstimate = (4061705, 594625.69, 50.62) 2024-11-06T04:49:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:49:16Z- TerrainNav:: SubcloudNIS is 0.32, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:49:16Z- TNavPF::Projecting Measurements 2024-11-06T04:49:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:49:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:49:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:49:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:49:16Z- TNavFilter::4 good beams. 2024-11-06T04:49:16Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:49:16Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:49:16Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:49:16Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:49:16Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:49:16Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:49:16Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:49:16Z- TerrainMapOctree: (vehN, vehE ) = (4061704.53, 594624.22). 2024-11-06T04:49:16Z- TerrainMapOctree: Min Distance = 133.75. 2024-11-06T04:49:16Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:49:16Z- Forcing Subcloud Comparison 2024-11-06T04:49:16Z- Weighting particles with subcloud comparison 2024-11-06T04:49:16Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:16Z- meanExpectedMeasDiff: 0.050157 delta_rms_squared: 2.754059 alpha: 0.992790 2024-11-06T04:49:16Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:16Z- meanExpectedMeasDiff: 0.383990 delta_rms_squared: 2.116810 alpha: 0.990640 2024-11-06T04:49:16Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:16Z- meanExpectedMeasDiff: 0.955455 delta_rms_squared: 1.249570 alpha: 0.984247 2024-11-06T04:49:16Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:16Z- meanExpectedMeasDiff: 0.248074 delta_rms_squared: 1.777440 alpha: 0.988873 2024-11-06T04:49:16Z- Subcloud NIS: 0.301827 2024-11-06T04:49:16Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:49:16Z- TNavPF:: sumWeights = 0.490731 2024-11-06T04:49:16Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:49:16Z- TNavPF::First Term of Map Covariance Matrix is 0.44 2024-11-06T04:49:16Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T04:49:16Z- TerrainNav::Resampling particle filter... 2024-11-06T04:49:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868554.37 sec. 2024-11-06T04:49:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:49:16Z- variances([0], [2], and psi_berg) = 0.429011 0.546214 0.000000 2024-11-06T04:49:16Z- publishEstimatesDecoder() - at 1730868555.98 sec 2024-11-06T04:49:16Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:49:16Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061648.27 594629.35 53.89 2024-11-06T04:49:16Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061648.34 594630.05 54.50 lat/lon: 36.70 -121.94 2024-11-06T04:49:16Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.43 0.55 0.69 0.00 2024-11-06T04:49:16Z- AHRS_M2 msg: 1730868556.37 epoch sec; seqNo:1938 2024-11-06T04:49:16Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.55 psi 2024-11-06T04:49:16Z- handleDepth()- Depth_Keller msg: 1730868556.38 epoch sec; seqNo:2077; depth 50.62 2024-11-06T04:49:16Z- handleDVL() - DVL msg epochMillisec: 1730868556.40 2024-11-06T04:49:16Z- handleDvl() - RDI_Pathfinder msg: 1730868556.40 sec; seqNo:761; #beams:4 2024-11-06T04:49:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.38 , 32.28 , 33.47 , 35.51 2024-11-06T04:49:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , -0.03 2024-11-06T04:49:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868556.41 epoch sec; seqNo:2077; 4061704.55 north; 594625.69 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T04:49:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868555.98 2024-11-06T04:49:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:16Z- waiting for 1730868559.98, time is 1730868556.41 2024-11-06T04:49:16Z- AHRS_M2 msg: 1730868556.78 epoch sec; seqNo:1939 2024-11-06T04:49:16Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.55 psi 2024-11-06T04:49:16Z- handleDepth()- Depth_Keller msg: 1730868556.80 epoch sec; seqNo:2078; depth 50.63 2024-11-06T04:49:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868556.82 epoch sec; seqNo:2078; 4061704.56 north; 594626.91 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T04:49:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868555.98 2024-11-06T04:49:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:16Z- waiting for 1730868559.98, time is 1730868556.82 2024-11-06T04:49:17Z- AHRS_M2 msg: 1730868557.18 epoch sec; seqNo:1940 2024-11-06T04:49:17Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.55 psi 2024-11-06T04:49:17Z- handleDepth()- Depth_Keller msg: 1730868557.18 epoch sec; seqNo:2079; depth 50.63 2024-11-06T04:49:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868557.19 epoch sec; seqNo:2079; 4061704.56 north; 594626.91 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T04:49:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868555.98 2024-11-06T04:49:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:17Z- waiting for 1730868559.98, time is 1730868557.19 2024-11-06T04:49:17Z- AHRS_M2 msg: 1730868557.58 epoch sec; seqNo:1941 2024-11-06T04:49:17Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.55 psi 2024-11-06T04:49:17Z- handleDepth()- Depth_Keller msg: 1730868557.59 epoch sec; seqNo:2080; depth 50.65 2024-11-06T04:49:17Z- handleDVL() - DVL msg epochMillisec: 1730868557.59 2024-11-06T04:49:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:17Z- handleDvl() - RDI_Pathfinder msg: 1730868557.59 sec; seqNo:762; #beams:0 2024-11-06T04:49:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.04 , -0.03 2024-11-06T04:49:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868557.60 epoch sec; seqNo:2080; 4061704.56 north; 594626.91 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T04:49:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868555.98 2024-11-06T04:49:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:17Z- waiting for 1730868559.98, time is 1730868557.60 2024-11-06T04:49:18Z- AHRS_M2 msg: 1730868557.99 epoch sec; seqNo:1942 2024-11-06T04:49:18Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.55 psi 2024-11-06T04:49:18Z- handleDepth()- Depth_Keller msg: 1730868558.00 epoch sec; seqNo:2081; depth 50.63 2024-11-06T04:49:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868558.01 epoch sec; seqNo:2081; 4061704.56 north; 594626.91 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T04:49:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868555.98 2024-11-06T04:49:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:18Z- waiting for 1730868559.98, time is 1730868558.01 2024-11-06T04:49:18Z- AHRS_M2 msg: 1730868558.39 epoch sec; seqNo:1943 2024-11-06T04:49:18Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.55 psi 2024-11-06T04:49:18Z- handleDepth()- Depth_Keller msg: 1730868558.40 epoch sec; seqNo:2082; depth 50.62 2024-11-06T04:49:18Z- handleDVL() - DVL msg epochMillisec: 1730868558.43 2024-11-06T04:49:18Z- handleDvl() - RDI_Pathfinder msg: 1730868558.43 sec; seqNo:763; #beams:4 2024-11-06T04:49:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.21 , 31.78 , 33.47 , 35.34 2024-11-06T04:49:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.07 , -0.05 2024-11-06T04:49:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868558.44 epoch sec; seqNo:2082; 4061704.95 north; 594628.12 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T04:49:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868555.98 2024-11-06T04:49:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:18Z- waiting for 1730868559.98, time is 1730868558.44 2024-11-06T04:49:18Z- AHRS_M2 msg: 1730868558.80 epoch sec; seqNo:1944 2024-11-06T04:49:18Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.55 psi 2024-11-06T04:49:18Z- handleDepth()- Depth_Keller msg: 1730868558.82 epoch sec; seqNo:2083; depth 50.63 2024-11-06T04:49:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868558.85 epoch sec; seqNo:2083; 4061704.95 north; 594628.12 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T04:49:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868555.98 2024-11-06T04:49:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:18Z- waiting for 1730868559.98, time is 1730868558.85 2024-11-06T04:49:19Z- AHRS_M2 msg: 1730868559.20 epoch sec; seqNo:1945 2024-11-06T04:49:19Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.55 psi 2024-11-06T04:49:19Z- handleDepth()- Depth_Keller msg: 1730868559.20 epoch sec; seqNo:2084; depth 50.61 2024-11-06T04:49:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868559.21 epoch sec; seqNo:2084; 4061704.95 north; 594628.12 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T04:49:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868555.98 2024-11-06T04:49:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:19Z- waiting for 1730868559.98, time is 1730868559.21 2024-11-06T04:49:19Z- AHRS_M2 msg: 1730868559.61 epoch sec; seqNo:1946 2024-11-06T04:49:19Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.54 psi 2024-11-06T04:49:19Z- handleDepth()- Depth_Keller msg: 1730868559.62 epoch sec; seqNo:2085; depth 50.63 2024-11-06T04:49:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868559.63 epoch sec; seqNo:2085; 4061704.97 north; 594629.35 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T04:49:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868555.98 2024-11-06T04:49:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:19Z- waiting for 1730868559.98, time is 1730868559.63 2024-11-06T04:49:20Z- AHRS_M2 msg: 1730868560.01 epoch sec; seqNo:1947 2024-11-06T04:49:20Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.54 psi 2024-11-06T04:49:20Z- handleDepth()- Depth_Keller msg: 1730868560.02 epoch sec; seqNo:2086; depth 50.61 2024-11-06T04:49:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868560.05 epoch sec; seqNo:2086; 4061704.97 north; 594629.35 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T04:49:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868555.98 2024-11-06T04:49:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:20Z- waiting for 1730868559.98, time is 1730868560.05 2024-11-06T04:49:20Z- LcmTrn::updateTrn() >>>> pose time 1730868560.05, meas time 1730868558.43 <<<< 2024-11-06T04:49:20Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868560.05 2024-11-06T04:49:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868558.43 sec, ping # 0; waiting for INS data more recent than 1730868555.98... 2024-11-06T04:49:20Z- TerrainNav::motionUpdate() - currEstimate = (4061705, 594629.35, 50.61) 2024-11-06T04:49:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:49:20Z- TerrainNav:: SubcloudNIS is 0.30, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:49:20Z- TNavPF::Projecting Measurements 2024-11-06T04:49:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:49:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:49:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:49:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:49:20Z- TNavFilter::4 good beams. 2024-11-06T04:49:20Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:49:20Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:49:20Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:49:20Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:49:20Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:49:20Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:49:20Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:49:20Z- TerrainMapOctree: (vehN, vehE ) = (4061704.80, 594627.89). 2024-11-06T04:49:20Z- TerrainMapOctree: Min Distance = 130.21. 2024-11-06T04:49:20Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:49:20Z- Forcing Subcloud Comparison 2024-11-06T04:49:20Z- Weighting particles with subcloud comparison 2024-11-06T04:49:20Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:20Z- meanExpectedMeasDiff: -0.044123 delta_rms_squared: 2.471881 alpha: 0.991974 2024-11-06T04:49:20Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:20Z- meanExpectedMeasDiff: 0.191895 delta_rms_squared: 1.169770 alpha: 0.983190 2024-11-06T04:49:20Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:20Z- meanExpectedMeasDiff: 0.981163 delta_rms_squared: 1.221321 alpha: 0.983888 2024-11-06T04:49:20Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:20Z- meanExpectedMeasDiff: 0.953372 delta_rms_squared: 1.571341 alpha: 0.987432 2024-11-06T04:49:20Z- Subcloud NIS: 0.283657 2024-11-06T04:49:20Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:49:20Z- TNavPF:: sumWeights = 0.491719 2024-11-06T04:49:20Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:49:20Z- TNavPF::First Term of Map Covariance Matrix is 0.57 2024-11-06T04:49:20Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T04:49:20Z- TerrainNav::Resampling particle filter... 2024-11-06T04:49:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868558.43 sec. 2024-11-06T04:49:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:49:20Z- variances([0], [2], and psi_berg) = 0.422974 0.552253 0.000000 2024-11-06T04:49:20Z- publishEstimatesDecoder() - at 1730868560.05 sec 2024-11-06T04:49:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:49:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061648.71 594632.94 53.61 2024-11-06T04:49:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061648.72 594633.76 54.26 lat/lon: 36.70 -121.94 2024-11-06T04:49:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.42 0.55 0.60 0.00 2024-11-06T04:49:20Z- AHRS_M2 msg: 1730868560.41 epoch sec; seqNo:1948 2024-11-06T04:49:20Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.54 psi 2024-11-06T04:49:20Z- handleDepth()- Depth_Keller msg: 1730868560.42 epoch sec; seqNo:2087; depth 50.61 2024-11-06T04:49:20Z- handleDVL() - DVL msg epochMillisec: 1730868560.44 2024-11-06T04:49:20Z- handleDvl() - RDI_Pathfinder msg: 1730868560.44 sec; seqNo:764; #beams:4 2024-11-06T04:49:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.21 , 31.78 , 33.47 , 35.00 2024-11-06T04:49:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , -0.02 2024-11-06T04:49:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868560.45 epoch sec; seqNo:2087; 4061704.97 north; 594629.35 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T04:49:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868560.05 2024-11-06T04:49:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:20Z- waiting for 1730868564.05, time is 1730868560.45 2024-11-06T04:49:20Z- AHRS_M2 msg: 1730868560.82 epoch sec; seqNo:1949 2024-11-06T04:49:20Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.54 psi 2024-11-06T04:49:20Z- handleDepth()- Depth_Keller msg: 1730868560.84 epoch sec; seqNo:2088; depth 50.59 2024-11-06T04:49:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868560.86 epoch sec; seqNo:2088; 4061704.97 north; 594629.35 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T04:49:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868560.05 2024-11-06T04:49:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:21Z- waiting for 1730868564.05, time is 1730868560.86 2024-11-06T04:49:21Z- AHRS_M2 msg: 1730868561.22 epoch sec; seqNo:1950 2024-11-06T04:49:21Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.54 psi 2024-11-06T04:49:21Z- handleDepth()- Depth_Keller msg: 1730868561.23 epoch sec; seqNo:2089; depth 50.58 2024-11-06T04:49:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868561.23 epoch sec; seqNo:2089; 4061704.98 north; 594630.57 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T04:49:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868560.05 2024-11-06T04:49:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:21Z- waiting for 1730868564.05, time is 1730868561.23 2024-11-06T04:49:21Z- AHRS_M2 msg: 1730868561.63 epoch sec; seqNo:1951 2024-11-06T04:49:21Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.54 psi 2024-11-06T04:49:21Z- handleDepth()- Depth_Keller msg: 1730868561.64 epoch sec; seqNo:2090; depth 50.59 2024-11-06T04:49:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868561.65 epoch sec; seqNo:2090; 4061704.98 north; 594630.57 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T04:49:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868560.05 2024-11-06T04:49:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:21Z- waiting for 1730868564.05, time is 1730868561.65 2024-11-06T04:49:22Z- AHRS_M2 msg: 1730868562.03 epoch sec; seqNo:1952 2024-11-06T04:49:22Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.54 psi 2024-11-06T04:49:22Z- handleDepth()- Depth_Keller msg: 1730868562.04 epoch sec; seqNo:2091; depth 50.59 2024-11-06T04:49:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868562.05 epoch sec; seqNo:2091; 4061704.98 north; 594630.57 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T04:49:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868560.05 2024-11-06T04:49:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:22Z- waiting for 1730868564.05, time is 1730868562.05 2024-11-06T04:49:22Z- AHRS_M2 msg: 1730868562.43 epoch sec; seqNo:1953 2024-11-06T04:49:22Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.54 psi 2024-11-06T04:49:22Z- handleDepth()- Depth_Keller msg: 1730868562.44 epoch sec; seqNo:2092; depth 50.59 2024-11-06T04:49:22Z- handleDVL() - DVL msg epochMillisec: 1730868562.47 2024-11-06T04:49:22Z- handleDvl() - RDI_Pathfinder msg: 1730868562.47 sec; seqNo:765; #beams:4 2024-11-06T04:49:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.47 , 31.50 , 33.34 , 34.56 2024-11-06T04:49:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.04 , 0.01 2024-11-06T04:49:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868562.48 epoch sec; seqNo:2092; 4061704.98 north; 594630.57 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T04:49:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868560.05 2024-11-06T04:49:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:22Z- waiting for 1730868564.05, time is 1730868562.48 2024-11-06T04:49:22Z- AHRS_M2 msg: 1730868562.84 epoch sec; seqNo:1954 2024-11-06T04:49:22Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.53 psi 2024-11-06T04:49:22Z- handleDepth()- Depth_Keller msg: 1730868562.85 epoch sec; seqNo:2093; depth 50.57 2024-11-06T04:49:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868562.87 epoch sec; seqNo:2093; 4061705.37 north; 594631.78 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T04:49:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868560.05 2024-11-06T04:49:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:23Z- waiting for 1730868564.05, time is 1730868562.87 2024-11-06T04:49:23Z- AHRS_M2 msg: 1730868563.24 epoch sec; seqNo:1955 2024-11-06T04:49:23Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.53 psi 2024-11-06T04:49:23Z- handleDepth()- Depth_Keller msg: 1730868563.24 epoch sec; seqNo:2094; depth 50.58 2024-11-06T04:49:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868563.25 epoch sec; seqNo:2094; 4061705.37 north; 594631.78 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T04:49:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868560.05 2024-11-06T04:49:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:23Z- waiting for 1730868564.05, time is 1730868563.25 2024-11-06T04:49:23Z- AHRS_M2 msg: 1730868563.64 epoch sec; seqNo:1956 2024-11-06T04:49:23Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.53 psi 2024-11-06T04:49:23Z- handleDepth()- Depth_Keller msg: 1730868563.65 epoch sec; seqNo:2095; depth 50.58 2024-11-06T04:49:23Z- handleDVL() - DVL msg epochMillisec: 1730868563.66 2024-11-06T04:49:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:23Z- handleDvl() - RDI_Pathfinder msg: 1730868563.66 sec; seqNo:766; #beams:0 2024-11-06T04:49:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.04 , 0.01 2024-11-06T04:49:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868563.67 epoch sec; seqNo:2095; 4061705.37 north; 594631.78 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T04:49:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868560.05 2024-11-06T04:49:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:23Z- waiting for 1730868564.05, time is 1730868563.67 2024-11-06T04:49:24Z- AHRS_M2 msg: 1730868564.05 epoch sec; seqNo:1957 2024-11-06T04:49:24Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.53 psi 2024-11-06T04:49:24Z- handleDepth()- Depth_Keller msg: 1730868564.06 epoch sec; seqNo:2096; depth 50.57 2024-11-06T04:49:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868564.07 epoch sec; seqNo:2096; 4061705.37 north; 594631.78 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T04:49:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868560.05 2024-11-06T04:49:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:24Z- waiting for 1730868564.05, time is 1730868564.07 2024-11-06T04:49:24Z- LcmTrn::updateTrn() >>>> pose time 1730868564.07, meas time 1730868563.66 <<<< 2024-11-06T04:49:24Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868564.07 2024-11-06T04:49:24Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:49:24Z- TerrainNav::motionUpdate() - currEstimate = (4061705, 594631.78, 50.57) 2024-11-06T04:49:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:49:24Z- TerrainNav:: SubcloudNIS is 0.28, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:49:24Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:49:24Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:49:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:49:24Z- variances([0], [2], and psi_berg) = 0.424327 0.553290 0.000000 2024-11-06T04:49:24Z- publishEstimatesDecoder() - at 1730868564.07 sec 2024-11-06T04:49:24Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:49:24Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061649.15 594635.36 53.70 2024-11-06T04:49:24Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061649.13 594636.20 54.22 lat/lon: 36.70 -121.94 2024-11-06T04:49:24Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.42 0.55 0.85 0.00 2024-11-06T04:49:24Z- AHRS_M2 msg: 1730868564.45 epoch sec; seqNo:1958 2024-11-06T04:49:24Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.53 psi 2024-11-06T04:49:24Z- handleDepth()- Depth_Keller msg: 1730868564.46 epoch sec; seqNo:2097; depth 50.58 2024-11-06T04:49:24Z- handleDVL() - DVL msg epochMillisec: 1730868564.49 2024-11-06T04:49:24Z- handleDvl() - RDI_Pathfinder msg: 1730868564.49 sec; seqNo:767; #beams:4 2024-11-06T04:49:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.01 , 31.50 , 33.19 , 34.56 2024-11-06T04:49:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , 0.03 2024-11-06T04:49:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868564.50 epoch sec; seqNo:2097; 4061705.38 north; 594633.00 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T04:49:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868564.07 2024-11-06T04:49:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:24Z- waiting for 1730868568.07, time is 1730868564.50 2024-11-06T04:49:24Z- AHRS_M2 msg: 1730868564.85 epoch sec; seqNo:1959 2024-11-06T04:49:24Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.53 psi 2024-11-06T04:49:24Z- handleDepth()- Depth_Keller msg: 1730868564.87 epoch sec; seqNo:2098; depth 50.57 2024-11-06T04:49:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868564.88 epoch sec; seqNo:2098; 4061705.38 north; 594633.00 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T04:49:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868564.07 2024-11-06T04:49:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:25Z- waiting for 1730868568.07, time is 1730868564.88 2024-11-06T04:49:25Z- AHRS_M2 msg: 1730868565.27 epoch sec; seqNo:1960 2024-11-06T04:49:25Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.53 psi 2024-11-06T04:49:25Z- handleDepth()- Depth_Keller msg: 1730868565.27 epoch sec; seqNo:2099; depth 50.57 2024-11-06T04:49:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868565.28 epoch sec; seqNo:2099; 4061705.38 north; 594633.00 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T04:49:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868564.07 2024-11-06T04:49:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:25Z- waiting for 1730868568.07, time is 1730868565.28 2024-11-06T04:49:25Z- AHRS_M2 msg: 1730868565.66 epoch sec; seqNo:1961 2024-11-06T04:49:25Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.53 psi 2024-11-06T04:49:25Z- handleDepth()- Depth_Keller msg: 1730868565.67 epoch sec; seqNo:2100; depth 50.55 2024-11-06T04:49:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868565.69 epoch sec; seqNo:2100; 4061705.40 north; 594634.22 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:49:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868564.07 2024-11-06T04:49:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:25Z- waiting for 1730868568.07, time is 1730868565.69 2024-11-06T04:49:26Z- handleDepth()- Depth_Keller msg: 1730868566.10 epoch sec; seqNo:2101; depth 50.56 2024-11-06T04:49:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868566.14 epoch sec; seqNo:2101; 4061705.40 north; 594634.22 east; 36.70 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T04:49:26Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:49:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868564.07 2024-11-06T04:49:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:26Z- waiting for 1730868568.07, time is 1730868566.14 2024-11-06T04:49:26Z- AHRS_M2 msg: 1730868566.47 epoch sec; seqNo:1962 2024-11-06T04:49:26Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.53 psi 2024-11-06T04:49:26Z- handleDepth()- Depth_Keller msg: 1730868566.48 epoch sec; seqNo:2102; depth 50.55 2024-11-06T04:49:26Z- handleDVL() - DVL msg epochMillisec: 1730868566.49 2024-11-06T04:49:26Z- handleDvl() - RDI_Pathfinder msg: 1730868566.49 sec; seqNo:768; #beams:4 2024-11-06T04:49:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.09 , 31.66 , 32.88 , 34.41 2024-11-06T04:49:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , 0.03 2024-11-06T04:49:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868566.52 epoch sec; seqNo:2102; 4061705.40 north; 594634.22 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:49:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868564.07 2024-11-06T04:49:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:26Z- waiting for 1730868568.07, time is 1730868566.52 2024-11-06T04:49:26Z- AHRS_M2 msg: 1730868566.88 epoch sec; seqNo:1963 2024-11-06T04:49:26Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.53 psi 2024-11-06T04:49:26Z- handleDepth()- Depth_Keller msg: 1730868566.89 epoch sec; seqNo:2103; depth 50.54 2024-11-06T04:49:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868566.90 epoch sec; seqNo:2103; 4061705.40 north; 594634.22 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T04:49:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868564.07 2024-11-06T04:49:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:27Z- waiting for 1730868568.07, time is 1730868566.90 2024-11-06T04:49:27Z- AHRS_M2 msg: 1730868567.28 epoch sec; seqNo:1964 2024-11-06T04:49:27Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.53 psi 2024-11-06T04:49:27Z- handleDepth()- Depth_Keller msg: 1730868567.29 epoch sec; seqNo:2104; depth 50.53 2024-11-06T04:49:27Z- handleDVL() - DVL msg epochMillisec: 1730868567.29 2024-11-06T04:49:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:27Z- handleDvl() - RDI_Pathfinder msg: 1730868567.29 sec; seqNo:769; #beams:0 2024-11-06T04:49:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.01 , 0.03 2024-11-06T04:49:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868567.30 epoch sec; seqNo:2104; 4061705.41 north; 594635.44 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T04:49:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868564.07 2024-11-06T04:49:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:27Z- waiting for 1730868568.07, time is 1730868567.30 2024-11-06T04:49:27Z- AHRS_M2 msg: 1730868567.69 epoch sec; seqNo:1965 2024-11-06T04:49:27Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.53 psi 2024-11-06T04:49:27Z- handleDepth()- Depth_Keller msg: 1730868567.69 epoch sec; seqNo:2105; depth 50.54 2024-11-06T04:49:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868567.70 epoch sec; seqNo:2105; 4061705.41 north; 594635.44 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T04:49:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868564.07 2024-11-06T04:49:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:27Z- waiting for 1730868568.07, time is 1730868567.70 2024-11-06T04:49:28Z- AHRS_M2 msg: 1730868568.11 epoch sec; seqNo:1966 2024-11-06T04:49:28Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:28Z- handleDepth()- Depth_Keller msg: 1730868568.13 epoch sec; seqNo:2106; depth 50.52 2024-11-06T04:49:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868568.16 epoch sec; seqNo:2106; 4061705.41 north; 594635.44 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T04:49:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868564.07 2024-11-06T04:49:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:28Z- waiting for 1730868568.07, time is 1730868568.16 2024-11-06T04:49:28Z- LcmTrn::updateTrn() >>>> pose time 1730868568.16, meas time 1730868567.29 <<<< 2024-11-06T04:49:28Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868568.16 2024-11-06T04:49:28Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:49:28Z- TerrainNav::motionUpdate() - currEstimate = (4061705, 594635.44, 50.52) 2024-11-06T04:49:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:49:28Z- TerrainNav:: SubcloudNIS is 0.28, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:49:28Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:49:28Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:49:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:49:28Z- variances([0], [2], and psi_berg) = 0.426221 0.555381 0.000000 2024-11-06T04:49:28Z- publishEstimatesDecoder() - at 1730868568.16 sec 2024-11-06T04:49:28Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:49:28Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061649.19 594639.01 54.02 2024-11-06T04:49:28Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061649.17 594639.86 54.16 lat/lon: 36.70 -121.94 2024-11-06T04:49:28Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.43 0.56 1.10 0.00 2024-11-06T04:49:28Z- AHRS_M2 msg: 1730868568.49 epoch sec; seqNo:1967 2024-11-06T04:49:28Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:28Z- handleDepth()- Depth_Keller msg: 1730868568.51 epoch sec; seqNo:2107; depth 50.52 2024-11-06T04:49:28Z- handleDVL() - DVL msg epochMillisec: 1730868568.53 2024-11-06T04:49:28Z- handleDvl() - RDI_Pathfinder msg: 1730868568.53 sec; seqNo:770; #beams:4 2024-11-06T04:49:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.17 , 31.66 , 32.73 , 34.26 2024-11-06T04:49:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , 0.01 2024-11-06T04:49:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868568.54 epoch sec; seqNo:2107; 4061705.42 north; 594636.66 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T04:49:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868568.16 2024-11-06T04:49:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:28Z- waiting for 1730868572.16, time is 1730868568.54 2024-11-06T04:49:28Z- AHRS_M2 msg: 1730868568.89 epoch sec; seqNo:1968 2024-11-06T04:49:28Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:28Z- handleDepth()- Depth_Keller msg: 1730868568.91 epoch sec; seqNo:2108; depth 50.50 2024-11-06T04:49:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868568.93 epoch sec; seqNo:2108; 4061705.42 north; 594636.66 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T04:49:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868568.16 2024-11-06T04:49:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:29Z- waiting for 1730868572.16, time is 1730868568.93 2024-11-06T04:49:29Z- AHRS_M2 msg: 1730868569.30 epoch sec; seqNo:1969 2024-11-06T04:49:29Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:29Z- handleDepth()- Depth_Keller msg: 1730868569.31 epoch sec; seqNo:2109; depth 50.48 2024-11-06T04:49:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868569.32 epoch sec; seqNo:2109; 4061705.42 north; 594636.66 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T04:49:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868568.16 2024-11-06T04:49:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:29Z- waiting for 1730868572.16, time is 1730868569.32 2024-11-06T04:49:29Z- AHRS_M2 msg: 1730868569.71 epoch sec; seqNo:1970 2024-11-06T04:49:29Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:29Z- handleDepth()- Depth_Keller msg: 1730868569.72 epoch sec; seqNo:2110; depth 50.46 2024-11-06T04:49:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868569.73 epoch sec; seqNo:2110; 4061705.42 north; 594636.66 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T04:49:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868568.16 2024-11-06T04:49:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:29Z- waiting for 1730868572.16, time is 1730868569.73 2024-11-06T04:49:30Z- AHRS_M2 msg: 1730868570.16 epoch sec; seqNo:1971 2024-11-06T04:49:30Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:30Z- handleDepth()- Depth_Keller msg: 1730868570.16 epoch sec; seqNo:2111; depth 50.45 2024-11-06T04:49:30Z- handleDVL() - DVL msg epochMillisec: 1730868570.19 2024-11-06T04:49:30Z- handleDvl() - RDI_Pathfinder msg: 1730868570.19 sec; seqNo:771; #beams:4 2024-11-06T04:49:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.48 , 31.66 , 33.65 , 33.95 2024-11-06T04:49:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , 0.00 2024-11-06T04:49:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868570.20 epoch sec; seqNo:2111; 4061705.44 north; 594637.88 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T04:49:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868568.16 2024-11-06T04:49:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:30Z- waiting for 1730868572.16, time is 1730868570.20 2024-11-06T04:49:30Z- AHRS_M2 msg: 1730868570.51 epoch sec; seqNo:1972 2024-11-06T04:49:30Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:30Z- handleDepth()- Depth_Keller msg: 1730868570.52 epoch sec; seqNo:2112; depth 50.44 2024-11-06T04:49:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868570.54 epoch sec; seqNo:2112; 4061705.44 north; 594637.88 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T04:49:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868568.16 2024-11-06T04:49:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:30Z- waiting for 1730868572.16, time is 1730868570.54 2024-11-06T04:49:30Z- AHRS_M2 msg: 1730868570.91 epoch sec; seqNo:1973 2024-11-06T04:49:30Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:30Z- handleDepth()- Depth_Keller msg: 1730868570.93 epoch sec; seqNo:2113; depth 50.43 2024-11-06T04:49:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868570.93 epoch sec; seqNo:2113; 4061705.82 north; 594637.88 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:49:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868568.16 2024-11-06T04:49:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:31Z- waiting for 1730868572.16, time is 1730868570.93 2024-11-06T04:49:31Z- AHRS_M2 msg: 1730868571.32 epoch sec; seqNo:1974 2024-11-06T04:49:31Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:31Z- handleDepth()- Depth_Keller msg: 1730868571.33 epoch sec; seqNo:2114; depth 50.42 2024-11-06T04:49:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868571.34 epoch sec; seqNo:2114; 4061705.83 north; 594639.10 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T04:49:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868568.16 2024-11-06T04:49:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:31Z- waiting for 1730868572.16, time is 1730868571.34 2024-11-06T04:49:31Z- AHRS_M2 msg: 1730868571.72 epoch sec; seqNo:1975 2024-11-06T04:49:31Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:31Z- handleDepth()- Depth_Keller msg: 1730868571.73 epoch sec; seqNo:2115; depth 50.40 2024-11-06T04:49:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868571.75 epoch sec; seqNo:2115; 4061705.83 north; 594639.10 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T04:49:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868568.16 2024-11-06T04:49:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:31Z- waiting for 1730868572.16, time is 1730868571.75 2024-11-06T04:49:32Z- AHRS_M2 msg: 1730868572.16 epoch sec; seqNo:1976 2024-11-06T04:49:32Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.53 psi 2024-11-06T04:49:32Z- handleDepth()- Depth_Keller msg: 1730868572.17 epoch sec; seqNo:2116; depth 50.37 2024-11-06T04:49:32Z- handleDVL() - DVL msg epochMillisec: 1730868572.19 2024-11-06T04:49:32Z- handleDvl() - RDI_Pathfinder msg: 1730868572.19 sec; seqNo:772; #beams:4 2024-11-06T04:49:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.17 , 31.50 , 33.49 , 33.80 2024-11-06T04:49:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , -0.01 2024-11-06T04:49:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868572.20 epoch sec; seqNo:2116; 4061705.83 north; 594639.10 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:49:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868568.16 2024-11-06T04:49:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:32Z- waiting for 1730868572.16, time is 1730868572.20 2024-11-06T04:49:32Z- LcmTrn::updateTrn() >>>> pose time 1730868572.20, meas time 1730868572.19 <<<< 2024-11-06T04:49:32Z- LcmTrn::updateTrn() equating pose 1730868572.19 and meas times 1730868572.19 2024-11-06T04:49:32Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868572.19 2024-11-06T04:49:32Z- TerrainNav::motionUpdate() - currEstimate = (4061706, 594639.10, 50.37) 2024-11-06T04:49:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:49:32Z- TerrainNav:: SubcloudNIS is 0.28, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:49:32Z- TNavPF::Projecting Measurements 2024-11-06T04:49:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:49:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:49:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:49:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:49:32Z- TNavFilter::4 good beams. 2024-11-06T04:49:32Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:49:32Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:49:32Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:49:32Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:49:32Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:49:32Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:49:32Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:49:32Z- TerrainMapOctree: (vehN, vehE ) = (4061705.83, 594639.10). 2024-11-06T04:49:32Z- TerrainMapOctree: Min Distance = 119.37. 2024-11-06T04:49:32Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:49:32Z- Forcing Subcloud Comparison 2024-11-06T04:49:32Z- Weighting particles with subcloud comparison 2024-11-06T04:49:32Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:32Z- meanExpectedMeasDiff: -1.320355 delta_rms_squared: 2.570385 alpha: 0.992279 2024-11-06T04:49:32Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:32Z- meanExpectedMeasDiff: 0.522022 delta_rms_squared: 1.555859 alpha: 0.987309 2024-11-06T04:49:32Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:32Z- meanExpectedMeasDiff: -0.323165 delta_rms_squared: 1.481894 alpha: 0.986683 2024-11-06T04:49:32Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:32Z- meanExpectedMeasDiff: -0.018477 delta_rms_squared: 2.064816 alpha: 0.990407 2024-11-06T04:49:32Z- Subcloud NIS: 0.261958 2024-11-06T04:49:32Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:49:32Z- TNavPF:: sumWeights = 0.441972 2024-11-06T04:49:32Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:49:32Z- TNavPF::First Term of Map Covariance Matrix is 0.48 2024-11-06T04:49:32Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T04:49:32Z- TerrainNav::Resampling particle filter... 2024-11-06T04:49:32Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868572.19 sec, ping # 0. 2024-11-06T04:49:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:49:32Z- variances([0], [2], and psi_berg) = 0.379683 0.514840 0.000000 2024-11-06T04:49:32Z- publishEstimatesDecoder() - at 1730868572.19 sec 2024-11-06T04:49:32Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:49:32Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061649.68 594642.69 54.06 2024-11-06T04:49:32Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061649.58 594643.53 54.18 lat/lon: 36.70 -121.94 2024-11-06T04:49:32Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.38 0.51 0.45 0.00 2024-11-06T04:49:32Z- AHRS_M2 msg: 1730868572.54 epoch sec; seqNo:1977 2024-11-06T04:49:32Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.53 psi 2024-11-06T04:49:32Z- handleDepth()- Depth_Keller msg: 1730868572.55 epoch sec; seqNo:2117; depth 50.34 2024-11-06T04:49:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868572.56 epoch sec; seqNo:2117; 4061705.83 north; 594639.10 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:49:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868572.19 2024-11-06T04:49:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:32Z- waiting for 1730868576.19, time is 1730868572.56 2024-11-06T04:49:32Z- AHRS_M2 msg: 1730868572.95 epoch sec; seqNo:1978 2024-11-06T04:49:32Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.53 psi 2024-11-06T04:49:32Z- handleDepth()- Depth_Keller msg: 1730868572.95 epoch sec; seqNo:2118; depth 50.34 2024-11-06T04:49:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868572.96 epoch sec; seqNo:2118; 4061705.84 north; 594640.32 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:49:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868572.19 2024-11-06T04:49:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:33Z- waiting for 1730868576.19, time is 1730868572.96 2024-11-06T04:49:33Z- AHRS_M2 msg: 1730868573.35 epoch sec; seqNo:1979 2024-11-06T04:49:33Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.53 psi 2024-11-06T04:49:33Z- handleDepth()- Depth_Keller msg: 1730868573.36 epoch sec; seqNo:2119; depth 50.32 2024-11-06T04:49:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868573.37 epoch sec; seqNo:2119; 4061705.84 north; 594640.32 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:49:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868572.19 2024-11-06T04:49:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:33Z- waiting for 1730868576.19, time is 1730868573.37 2024-11-06T04:49:33Z- AHRS_M2 msg: 1730868573.76 epoch sec; seqNo:1980 2024-11-06T04:49:33Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.53 psi 2024-11-06T04:49:33Z- handleDepth()- Depth_Keller msg: 1730868573.77 epoch sec; seqNo:2120; depth 50.29 2024-11-06T04:49:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868573.79 epoch sec; seqNo:2120; 4061705.84 north; 594640.32 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:49:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868572.19 2024-11-06T04:49:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:33Z- waiting for 1730868576.19, time is 1730868573.79 2024-11-06T04:49:34Z- AHRS_M2 msg: 1730868574.16 epoch sec; seqNo:1981 2024-11-06T04:49:34Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.53 psi 2024-11-06T04:49:34Z- handleDepth()- Depth_Keller msg: 1730868574.17 epoch sec; seqNo:2121; depth 50.28 2024-11-06T04:49:34Z- handleDVL() - DVL msg epochMillisec: 1730868574.19 2024-11-06T04:49:34Z- handleDvl() - RDI_Pathfinder msg: 1730868574.19 sec; seqNo:773; #beams:4 2024-11-06T04:49:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.63 , 31.66 , 34.10 , 33.95 2024-11-06T04:49:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , -0.03 2024-11-06T04:49:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868574.21 epoch sec; seqNo:2121; 4061705.86 north; 594641.54 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T04:49:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868572.19 2024-11-06T04:49:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:34Z- waiting for 1730868576.19, time is 1730868574.21 2024-11-06T04:49:34Z- AHRS_M2 msg: 1730868574.56 epoch sec; seqNo:1982 2024-11-06T04:49:34Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.53 psi 2024-11-06T04:49:34Z- handleDepth()- Depth_Keller msg: 1730868574.57 epoch sec; seqNo:2122; depth 50.25 2024-11-06T04:49:34Z- handleDVL() - DVL msg epochMillisec: 1730868574.57 2024-11-06T04:49:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:34Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:34Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:34Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:34Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:34Z- handleDvl() - RDI_Pathfinder msg: 1730868574.57 sec; seqNo:774; #beams:0 2024-11-06T04:49:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , -0.03 2024-11-06T04:49:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868574.59 epoch sec; seqNo:2122; 4061705.86 north; 594641.54 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T04:49:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868572.19 2024-11-06T04:49:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:34Z- waiting for 1730868576.19, time is 1730868574.59 2024-11-06T04:49:34Z- AHRS_M2 msg: 1730868574.97 epoch sec; seqNo:1983 2024-11-06T04:49:34Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.53 psi 2024-11-06T04:49:34Z- handleDepth()- Depth_Keller msg: 1730868574.98 epoch sec; seqNo:2123; depth 50.24 2024-11-06T04:49:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868574.99 epoch sec; seqNo:2123; 4061705.86 north; 594641.54 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:49:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868572.19 2024-11-06T04:49:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:35Z- waiting for 1730868576.19, time is 1730868574.99 2024-11-06T04:49:35Z- AHRS_M2 msg: 1730868575.37 epoch sec; seqNo:1984 2024-11-06T04:49:35Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.53 psi 2024-11-06T04:49:35Z- handleDepth()- Depth_Keller msg: 1730868575.38 epoch sec; seqNo:2124; depth 50.21 2024-11-06T04:49:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868575.41 epoch sec; seqNo:2124; 4061705.86 north; 594641.54 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T04:49:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868572.19 2024-11-06T04:49:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:35Z- waiting for 1730868576.19, time is 1730868575.41 2024-11-06T04:49:35Z- AHRS_M2 msg: 1730868575.77 epoch sec; seqNo:1985 2024-11-06T04:49:35Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.53 psi 2024-11-06T04:49:35Z- handleDepth()- Depth_Keller msg: 1730868575.78 epoch sec; seqNo:2125; depth 50.20 2024-11-06T04:49:35Z- handleDVL() - DVL msg epochMillisec: 1730868575.79 2024-11-06T04:49:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:35Z- handleDvl() - RDI_Pathfinder msg: 1730868575.79 sec; seqNo:775; #beams:0 2024-11-06T04:49:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , -0.03 2024-11-06T04:49:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868575.83 epoch sec; seqNo:2125; 4061706.25 north; 594642.76 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:49:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868572.19 2024-11-06T04:49:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:35Z- waiting for 1730868576.19, time is 1730868575.83 2024-11-06T04:49:36Z- AHRS_M2 msg: 1730868576.18 epoch sec; seqNo:1986 2024-11-06T04:49:36Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.53 psi 2024-11-06T04:49:36Z- handleDepth()- Depth_Keller msg: 1730868576.18 epoch sec; seqNo:2126; depth 50.20 2024-11-06T04:49:36Z- handleDVL() - DVL msg epochMillisec: 1730868576.21 2024-11-06T04:49:36Z- handleDvl() - RDI_Pathfinder msg: 1730868576.21 sec; seqNo:776; #beams:4 2024-11-06T04:49:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.31 , 31.66 , 34.41 , 33.95 2024-11-06T04:49:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.06 , -0.02 2024-11-06T04:49:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868576.22 epoch sec; seqNo:2126; 4061706.25 north; 594642.76 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:49:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868572.19 2024-11-06T04:49:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:36Z- waiting for 1730868576.19, time is 1730868576.22 2024-11-06T04:49:36Z- LcmTrn::updateTrn() >>>> pose time 1730868576.22, meas time 1730868576.21 <<<< 2024-11-06T04:49:36Z- LcmTrn::updateTrn() equating pose 1730868576.21 and meas times 1730868576.21 2024-11-06T04:49:36Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868576.21 2024-11-06T04:49:36Z- TerrainNav::motionUpdate() - currEstimate = (4061706, 594642.76, 50.20) 2024-11-06T04:49:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:49:36Z- TerrainNav:: SubcloudNIS is 0.26, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:49:36Z- TNavPF::Projecting Measurements 2024-11-06T04:49:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:49:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:49:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:49:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:49:36Z- TNavFilter::4 good beams. 2024-11-06T04:49:36Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:49:36Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:49:36Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:49:36Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:49:36Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:49:36Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:49:36Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:49:36Z- TerrainMapOctree: (vehN, vehE ) = (4061706.25, 594642.76). 2024-11-06T04:49:36Z- TerrainMapOctree: Min Distance = 115.82. 2024-11-06T04:49:36Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:49:36Z- Forcing Subcloud Comparison 2024-11-06T04:49:36Z- Weighting particles with subcloud comparison 2024-11-06T04:49:36Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:36Z- meanExpectedMeasDiff: 1.121282 delta_rms_squared: 1.129012 alpha: 0.982594 2024-11-06T04:49:36Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:36Z- meanExpectedMeasDiff: 0.922536 delta_rms_squared: 0.790294 alpha: 0.975318 2024-11-06T04:49:36Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:36Z- meanExpectedMeasDiff: 1.434805 delta_rms_squared: 0.943552 alpha: 0.979243 2024-11-06T04:49:36Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:36Z- meanExpectedMeasDiff: 0.720914 delta_rms_squared: 1.346681 alpha: 0.985366 2024-11-06T04:49:36Z- Subcloud NIS: 0.241388 2024-11-06T04:49:36Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:49:36Z- TNavPF:: sumWeights = 0.467624 2024-11-06T04:49:36Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:49:36Z- TNavPF::First Term of Map Covariance Matrix is 0.32 2024-11-06T04:49:36Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T04:49:36Z- TerrainNav::Resampling particle filter... 2024-11-06T04:49:36Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868576.21 sec, ping # 0. 2024-11-06T04:49:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:49:36Z- variances([0], [2], and psi_berg) = 0.386670 0.534387 0.000000 2024-11-06T04:49:36Z- publishEstimatesDecoder() - at 1730868576.21 sec 2024-11-06T04:49:36Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:49:36Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061650.06 594646.35 53.59 2024-11-06T04:49:36Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061650.02 594647.24 53.57 lat/lon: 36.70 -121.94 2024-11-06T04:49:36Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.39 0.53 0.36 0.00 2024-11-06T04:49:36Z- AHRS_M2 msg: 1730868576.58 epoch sec; seqNo:1987 2024-11-06T04:49:36Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 1.53 psi 2024-11-06T04:49:36Z- handleDepth()- Depth_Keller msg: 1730868576.59 epoch sec; seqNo:2127; depth 50.16 2024-11-06T04:49:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868576.60 epoch sec; seqNo:2127; 4061706.25 north; 594642.76 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T04:49:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868576.21 2024-11-06T04:49:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:36Z- waiting for 1730868580.21, time is 1730868576.60 2024-11-06T04:49:37Z- handleDepth()- Depth_Keller msg: 1730868577.01 epoch sec; seqNo:2128; depth 50.15 2024-11-06T04:49:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868577.02 epoch sec; seqNo:2128; 4061706.25 north; 594642.76 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T04:49:37Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:49:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868576.21 2024-11-06T04:49:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:37Z- waiting for 1730868580.21, time is 1730868577.02 2024-11-06T04:49:37Z- AHRS_M2 msg: 1730868577.39 epoch sec; seqNo:1988 2024-11-06T04:49:37Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:37Z- handleDepth()- Depth_Keller msg: 1730868577.40 epoch sec; seqNo:2129; depth 50.13 2024-11-06T04:49:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868577.41 epoch sec; seqNo:2129; 4061706.26 north; 594643.98 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T04:49:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868576.21 2024-11-06T04:49:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:37Z- waiting for 1730868580.21, time is 1730868577.41 2024-11-06T04:49:37Z- AHRS_M2 msg: 1730868577.80 epoch sec; seqNo:1989 2024-11-06T04:49:37Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:37Z- handleDepth()- Depth_Keller msg: 1730868577.80 epoch sec; seqNo:2130; depth 50.12 2024-11-06T04:49:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868577.85 epoch sec; seqNo:2130; 4061706.26 north; 594643.98 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T04:49:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868576.21 2024-11-06T04:49:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:37Z- waiting for 1730868580.21, time is 1730868577.85 2024-11-06T04:49:38Z- AHRS_M2 msg: 1730868578.20 epoch sec; seqNo:1990 2024-11-06T04:49:38Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:38Z- handleDepth()- Depth_Keller msg: 1730868578.21 epoch sec; seqNo:2131; depth 50.11 2024-11-06T04:49:38Z- handleDVL() - DVL msg epochMillisec: 1730868578.23 2024-11-06T04:49:38Z- handleDvl() - RDI_Pathfinder msg: 1730868578.23 sec; seqNo:777; #beams:4 2024-11-06T04:49:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.23 , 31.50 , 34.26 , 33.95 2024-11-06T04:49:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , 0.01 2024-11-06T04:49:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868578.26 epoch sec; seqNo:2131; 4061706.26 north; 594643.98 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T04:49:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868576.21 2024-11-06T04:49:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:38Z- waiting for 1730868580.21, time is 1730868578.26 2024-11-06T04:49:38Z- AHRS_M2 msg: 1730868578.60 epoch sec; seqNo:1991 2024-11-06T04:49:38Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:38Z- handleDepth()- Depth_Keller msg: 1730868578.61 epoch sec; seqNo:2132; depth 50.09 2024-11-06T04:49:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868578.63 epoch sec; seqNo:2132; 4061706.28 north; 594645.20 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T04:49:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868576.21 2024-11-06T04:49:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:38Z- waiting for 1730868580.21, time is 1730868578.63 2024-11-06T04:49:39Z- AHRS_M2 msg: 1730868579.01 epoch sec; seqNo:1992 2024-11-06T04:49:39Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:39Z- handleDepth()- Depth_Keller msg: 1730868579.02 epoch sec; seqNo:2133; depth 50.08 2024-11-06T04:49:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868579.03 epoch sec; seqNo:2133; 4061706.28 north; 594645.20 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T04:49:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868576.21 2024-11-06T04:49:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:39Z- waiting for 1730868580.21, time is 1730868579.03 2024-11-06T04:49:39Z- AHRS_M2 msg: 1730868579.41 epoch sec; seqNo:1993 2024-11-06T04:49:39Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:39Z- handleDepth()- Depth_Keller msg: 1730868579.42 epoch sec; seqNo:2134; depth 50.05 2024-11-06T04:49:39Z- handleDVL() - DVL msg epochMillisec: 1730868579.42 2024-11-06T04:49:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:39Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:39Z- handleDvl() - RDI_Pathfinder msg: 1730868579.42 sec; seqNo:778; #beams:0 2024-11-06T04:49:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.03 , 0.01 2024-11-06T04:49:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868579.44 epoch sec; seqNo:2134; 4061706.28 north; 594645.20 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T04:49:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868576.21 2024-11-06T04:49:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:39Z- waiting for 1730868580.21, time is 1730868579.44 2024-11-06T04:49:39Z- handleDepth()- Depth_Keller msg: 1730868579.84 epoch sec; seqNo:2135; depth 50.04 2024-11-06T04:49:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868579.86 epoch sec; seqNo:2135; 4061706.28 north; 594645.20 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T04:49:40Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:49:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868576.21 2024-11-06T04:49:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:40Z- waiting for 1730868580.21, time is 1730868579.86 2024-11-06T04:49:40Z- AHRS_M2 msg: 1730868580.22 epoch sec; seqNo:1994 2024-11-06T04:49:40Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:40Z- handleDepth()- Depth_Keller msg: 1730868580.23 epoch sec; seqNo:2136; depth 50.02 2024-11-06T04:49:40Z- handleDVL() - DVL msg epochMillisec: 1730868580.25 2024-11-06T04:49:40Z- handleDvl() - RDI_Pathfinder msg: 1730868580.25 sec; seqNo:779; #beams:4 2024-11-06T04:49:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.91 , 31.78 , 34.49 , 34.15 2024-11-06T04:49:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.00 , 0.04 2024-11-06T04:49:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868580.26 epoch sec; seqNo:2136; 4061706.29 north; 594646.42 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:49:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868576.21 2024-11-06T04:49:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:40Z- waiting for 1730868580.21, time is 1730868580.26 2024-11-06T04:49:40Z- LcmTrn::updateTrn() >>>> pose time 1730868580.26, meas time 1730868580.25 <<<< 2024-11-06T04:49:40Z- LcmTrn::updateTrn() equating pose 1730868580.25 and meas times 1730868580.25 2024-11-06T04:49:40Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868580.25 2024-11-06T04:49:40Z- TerrainNav::motionUpdate() - currEstimate = (4061706, 594646.42, 50.02) 2024-11-06T04:49:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:49:40Z- TerrainNav:: SubcloudNIS is 0.24, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:49:40Z- TNavPF::Projecting Measurements 2024-11-06T04:49:40Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:49:40Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:49:40Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:49:40Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:49:40Z- TNavFilter::4 good beams. 2024-11-06T04:49:40Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:49:40Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:49:40Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:49:40Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:49:40Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:49:40Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:49:40Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:49:40Z- TerrainMapOctree: (vehN, vehE ) = (4061706.29, 594646.42). 2024-11-06T04:49:40Z- TerrainMapOctree: Min Distance = 112.43. 2024-11-06T04:49:40Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:49:40Z- Forcing Subcloud Comparison 2024-11-06T04:49:40Z- Weighting particles with subcloud comparison 2024-11-06T04:49:40Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:40Z- meanExpectedMeasDiff: 2.001912 delta_rms_squared: 0.970448 alpha: 0.979807 2024-11-06T04:49:40Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:40Z- meanExpectedMeasDiff: 0.063846 delta_rms_squared: 0.718819 alpha: 0.972930 2024-11-06T04:49:40Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:40Z- meanExpectedMeasDiff: 1.476339 delta_rms_squared: 0.662512 alpha: 0.970696 2024-11-06T04:49:40Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:40Z- meanExpectedMeasDiff: -0.423810 delta_rms_squared: 0.778826 alpha: 0.974963 2024-11-06T04:49:40Z- Subcloud NIS: 0.220565 2024-11-06T04:49:40Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:49:40Z- TNavPF:: sumWeights = 0.376517 2024-11-06T04:49:40Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:49:40Z- TNavPF::First Term of Map Covariance Matrix is 0.61 2024-11-06T04:49:40Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T04:49:40Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868580.25 sec, ping # 0. 2024-11-06T04:49:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:49:40Z- variances([0], [2], and psi_berg) = 0.379578 0.524908 0.000000 2024-11-06T04:49:40Z- publishEstimatesDecoder() - at 1730868580.25 sec 2024-11-06T04:49:40Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:49:40Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061649.19 594649.49 52.63 2024-11-06T04:49:40Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061650.00 594650.86 53.12 lat/lon: 36.70 -121.94 2024-11-06T04:49:40Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.38 0.52 0.36 0.00 2024-11-06T04:49:40Z- AHRS_M2 msg: 1730868580.62 epoch sec; seqNo:1995 2024-11-06T04:49:40Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:40Z- handleDepth()- Depth_Keller msg: 1730868580.63 epoch sec; seqNo:2137; depth 50.02 2024-11-06T04:49:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868580.65 epoch sec; seqNo:2137; 4061706.29 north; 594646.42 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:49:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868580.25 2024-11-06T04:49:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:40Z- waiting for 1730868584.25, time is 1730868580.65 2024-11-06T04:49:41Z- AHRS_M2 msg: 1730868581.04 epoch sec; seqNo:1996 2024-11-06T04:49:41Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.52 psi 2024-11-06T04:49:41Z- handleDepth()- Depth_Keller msg: 1730868581.04 epoch sec; seqNo:2138; depth 50.01 2024-11-06T04:49:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868581.05 epoch sec; seqNo:2138; 4061706.29 north; 594646.42 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:49:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868580.25 2024-11-06T04:49:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:41Z- waiting for 1730868584.25, time is 1730868581.05 2024-11-06T04:49:41Z- AHRS_M2 msg: 1730868581.43 epoch sec; seqNo:1997 2024-11-06T04:49:41Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.52 psi 2024-11-06T04:49:41Z- handleDepth()- Depth_Keller msg: 1730868581.44 epoch sec; seqNo:2139; depth 50.00 2024-11-06T04:49:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868581.45 epoch sec; seqNo:2139; 4061706.67 north; 594646.41 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T04:49:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868580.25 2024-11-06T04:49:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:41Z- waiting for 1730868584.25, time is 1730868581.45 2024-11-06T04:49:41Z- AHRS_M2 msg: 1730868581.84 epoch sec; seqNo:1998 2024-11-06T04:49:41Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.52 psi 2024-11-06T04:49:41Z- handleDepth()- Depth_Keller msg: 1730868581.85 epoch sec; seqNo:2140; depth 49.97 2024-11-06T04:49:41Z- handleDVL() - DVL msg epochMillisec: 1730868581.85 2024-11-06T04:49:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:41Z- handleDvl() - RDI_Pathfinder msg: 1730868581.85 sec; seqNo:780; #beams:0 2024-11-06T04:49:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.00 , 0.04 2024-11-06T04:49:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868581.86 epoch sec; seqNo:2140; 4061706.68 north; 594647.63 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T04:49:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868580.25 2024-11-06T04:49:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:42Z- waiting for 1730868584.25, time is 1730868581.86 2024-11-06T04:49:42Z- AHRS_M2 msg: 1730868582.25 epoch sec; seqNo:1999 2024-11-06T04:49:42Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.52 psi 2024-11-06T04:49:42Z- handleDepth()- Depth_Keller msg: 1730868582.25 epoch sec; seqNo:2141; depth 49.96 2024-11-06T04:49:42Z- handleDVL() - DVL msg epochMillisec: 1730868582.27 2024-11-06T04:49:42Z- handleDvl() - RDI_Pathfinder msg: 1730868582.27 sec; seqNo:781; #beams:4 2024-11-06T04:49:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.40 , 31.94 , 35.00 , 34.49 2024-11-06T04:49:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.03 , 0.01 2024-11-06T04:49:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868582.30 epoch sec; seqNo:2141; 4061706.68 north; 594647.63 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:49:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868580.25 2024-11-06T04:49:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:42Z- waiting for 1730868584.25, time is 1730868582.30 2024-11-06T04:49:42Z- AHRS_M2 msg: 1730868582.64 epoch sec; seqNo:2000 2024-11-06T04:49:42Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.52 psi 2024-11-06T04:49:42Z- handleDepth()- Depth_Keller msg: 1730868582.66 epoch sec; seqNo:2142; depth 49.93 2024-11-06T04:49:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868582.67 epoch sec; seqNo:2142; 4061706.68 north; 594647.63 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T04:49:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868580.25 2024-11-06T04:49:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:42Z- waiting for 1730868584.25, time is 1730868582.67 2024-11-06T04:49:43Z- AHRS_M2 msg: 1730868583.05 epoch sec; seqNo:2001 2024-11-06T04:49:43Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.52 psi 2024-11-06T04:49:43Z- handleDepth()- Depth_Keller msg: 1730868583.05 epoch sec; seqNo:2143; depth 49.93 2024-11-06T04:49:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868583.06 epoch sec; seqNo:2143; 4061706.70 north; 594648.85 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T04:49:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868580.25 2024-11-06T04:49:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:43Z- waiting for 1730868584.25, time is 1730868583.06 2024-11-06T04:49:43Z- AHRS_M2 msg: 1730868583.45 epoch sec; seqNo:2002 2024-11-06T04:49:43Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.52 psi 2024-11-06T04:49:43Z- handleDepth()- Depth_Keller msg: 1730868583.46 epoch sec; seqNo:2144; depth 49.90 2024-11-06T04:49:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868583.47 epoch sec; seqNo:2144; 4061706.70 north; 594648.85 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:49:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868580.25 2024-11-06T04:49:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:43Z- waiting for 1730868584.25, time is 1730868583.47 2024-11-06T04:49:43Z- AHRS_M2 msg: 1730868583.86 epoch sec; seqNo:2003 2024-11-06T04:49:43Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.52 psi 2024-11-06T04:49:43Z- handleDepth()- Depth_Keller msg: 1730868583.86 epoch sec; seqNo:2145; depth 49.88 2024-11-06T04:49:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868583.88 epoch sec; seqNo:2145; 4061706.70 north; 594648.85 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T04:49:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868580.25 2024-11-06T04:49:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:44Z- waiting for 1730868584.25, time is 1730868583.88 2024-11-06T04:49:44Z- AHRS_M2 msg: 1730868584.26 epoch sec; seqNo:2004 2024-11-06T04:49:44Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:44Z- handleDepth()- Depth_Keller msg: 1730868584.27 epoch sec; seqNo:2146; depth 49.88 2024-11-06T04:49:44Z- handleDVL() - DVL msg epochMillisec: 1730868584.29 2024-11-06T04:49:44Z- handleDvl() - RDI_Pathfinder msg: 1730868584.29 sec; seqNo:782; #beams:4 2024-11-06T04:49:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.97 , 31.94 , 36.02 , 34.15 2024-11-06T04:49:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.03 2024-11-06T04:49:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868584.31 epoch sec; seqNo:2146; 4061706.70 north; 594648.85 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T04:49:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868580.25 2024-11-06T04:49:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:44Z- waiting for 1730868584.25, time is 1730868584.31 2024-11-06T04:49:44Z- LcmTrn::updateTrn() >>>> pose time 1730868584.31, meas time 1730868584.29 <<<< 2024-11-06T04:49:44Z- LcmTrn::updateTrn() equating pose 1730868584.29 and meas times 1730868584.29 2024-11-06T04:49:44Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868584.29 2024-11-06T04:49:44Z- TerrainNav::motionUpdate() - currEstimate = (4061707, 594648.85, 49.88) 2024-11-06T04:49:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:49:44Z- TerrainNav:: SubcloudNIS is 0.22, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:49:44Z- TNavPF::Projecting Measurements 2024-11-06T04:49:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:49:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:49:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:49:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:49:44Z- TNavFilter::4 good beams. 2024-11-06T04:49:44Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:49:44Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:49:44Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:49:44Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:49:44Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:49:44Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:49:44Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:49:44Z- TerrainMapOctree: (vehN, vehE ) = (4061706.70, 594648.85). 2024-11-06T04:49:44Z- TerrainMapOctree: Min Distance = 110.03. 2024-11-06T04:49:44Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:49:44Z- Forcing Subcloud Comparison 2024-11-06T04:49:44Z- Weighting particles with subcloud comparison 2024-11-06T04:49:44Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:44Z- meanExpectedMeasDiff: 4.467050 delta_rms_squared: 0.950918 alpha: 0.979401 2024-11-06T04:49:44Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:44Z- meanExpectedMeasDiff: -0.016488 delta_rms_squared: 0.770977 alpha: 0.974715 2024-11-06T04:49:44Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:44Z- meanExpectedMeasDiff: 3.169416 delta_rms_squared: 0.826351 alpha: 0.976369 2024-11-06T04:49:44Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:44Z- meanExpectedMeasDiff: -1.015109 delta_rms_squared: 0.780635 alpha: 0.975020 2024-11-06T04:49:44Z- Subcloud NIS: 0.234218 2024-11-06T04:49:44Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:49:44Z- TNavPF:: sumWeights = 0.035613 2024-11-06T04:49:44Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:49:44Z- TNavPF::First Term of Map Covariance Matrix is 0.47 2024-11-06T04:49:44Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T04:49:44Z- TerrainNav::Resampling particle filter... 2024-11-06T04:49:44Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868584.29 sec, ping # 0. 2024-11-06T04:49:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:49:44Z- variances([0], [2], and psi_berg) = 0.369670 0.517425 0.000000 2024-11-06T04:49:44Z- publishEstimatesDecoder() - at 1730868584.29 sec 2024-11-06T04:49:44Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:49:44Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061650.54 594652.40 52.93 2024-11-06T04:49:44Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061650.37 594653.34 52.37 lat/lon: 36.70 -121.94 2024-11-06T04:49:44Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.37 0.52 0.34 0.00 2024-11-06T04:49:44Z- AHRS_M2 msg: 1730868584.66 epoch sec; seqNo:2005 2024-11-06T04:49:44Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:44Z- handleDepth()- Depth_Keller msg: 1730868584.67 epoch sec; seqNo:2147; depth 49.87 2024-11-06T04:49:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868584.70 epoch sec; seqNo:2147; 4061706.71 north; 594650.07 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:49:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868584.29 2024-11-06T04:49:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:44Z- waiting for 1730868588.29, time is 1730868584.70 2024-11-06T04:49:45Z- AHRS_M2 msg: 1730868585.07 epoch sec; seqNo:2006 2024-11-06T04:49:45Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 1.53 psi 2024-11-06T04:49:45Z- handleDepth()- Depth_Keller msg: 1730868585.08 epoch sec; seqNo:2148; depth 49.84 2024-11-06T04:49:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868585.09 epoch sec; seqNo:2148; 4061706.71 north; 594650.07 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T04:49:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868584.29 2024-11-06T04:49:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:45Z- waiting for 1730868588.29, time is 1730868585.09 2024-11-06T04:49:45Z- AHRS_M2 msg: 1730868585.47 epoch sec; seqNo:2007 2024-11-06T04:49:45Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.53 psi 2024-11-06T04:49:45Z- handleDepth()- Depth_Keller msg: 1730868585.48 epoch sec; seqNo:2149; depth 49.83 2024-11-06T04:49:45Z- handleDVL() - DVL msg epochMillisec: 1730868585.48 2024-11-06T04:49:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:45Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:45Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:45Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:45Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:45Z- handleDvl() - RDI_Pathfinder msg: 1730868585.48 sec; seqNo:783; #beams:0 2024-11-06T04:49:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.03 2024-11-06T04:49:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868585.49 epoch sec; seqNo:2149; 4061706.71 north; 594650.07 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:49:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868584.29 2024-11-06T04:49:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:45Z- waiting for 1730868588.29, time is 1730868585.49 2024-11-06T04:49:45Z- AHRS_M2 msg: 1730868585.87 epoch sec; seqNo:2008 2024-11-06T04:49:45Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.52 psi 2024-11-06T04:49:45Z- handleDepth()- Depth_Keller msg: 1730868585.89 epoch sec; seqNo:2150; depth 49.81 2024-11-06T04:49:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868585.91 epoch sec; seqNo:2150; 4061706.72 north; 594651.29 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:49:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868584.29 2024-11-06T04:49:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:46Z- waiting for 1730868588.29, time is 1730868585.91 2024-11-06T04:49:46Z- AHRS_M2 msg: 1730868586.29 epoch sec; seqNo:2009 2024-11-06T04:49:46Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.52 psi 2024-11-06T04:49:46Z- handleDepth()- Depth_Keller msg: 1730868586.29 epoch sec; seqNo:2151; depth 49.81 2024-11-06T04:49:46Z- handleDVL() - DVL msg epochMillisec: 1730868586.32 2024-11-06T04:49:46Z- handleDvl() - RDI_Pathfinder msg: 1730868586.32 sec; seqNo:784; #beams:4 2024-11-06T04:49:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.80 , 31.78 , 36.19 , 34.32 2024-11-06T04:49:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , -0.03 2024-11-06T04:49:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868586.33 epoch sec; seqNo:2151; 4061706.72 north; 594651.29 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:49:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868584.29 2024-11-06T04:49:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:46Z- waiting for 1730868588.29, time is 1730868586.33 2024-11-06T04:49:46Z- AHRS_M2 msg: 1730868586.69 epoch sec; seqNo:2010 2024-11-06T04:49:46Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.52 psi 2024-11-06T04:49:46Z- handleDepth()- Depth_Keller msg: 1730868586.69 epoch sec; seqNo:2152; depth 49.78 2024-11-06T04:49:46Z- handleDVL() - DVL msg epochMillisec: 1730868586.70 2024-11-06T04:49:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:46Z- handleDvl() - RDI_Pathfinder msg: 1730868586.70 sec; seqNo:785; #beams:0 2024-11-06T04:49:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , -0.03 2024-11-06T04:49:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868586.71 epoch sec; seqNo:2152; 4061706.72 north; 594651.29 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:49:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868584.29 2024-11-06T04:49:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:46Z- waiting for 1730868588.29, time is 1730868586.71 2024-11-06T04:49:47Z- AHRS_M2 msg: 1730868587.09 epoch sec; seqNo:2011 2024-11-06T04:49:47Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 1.52 psi 2024-11-06T04:49:47Z- handleDepth()- Depth_Keller msg: 1730868587.09 epoch sec; seqNo:2153; depth 49.78 2024-11-06T04:49:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868587.11 epoch sec; seqNo:2153; 4061706.72 north; 594651.29 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:49:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868584.29 2024-11-06T04:49:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:47Z- waiting for 1730868588.29, time is 1730868587.11 2024-11-06T04:49:47Z- AHRS_M2 msg: 1730868587.49 epoch sec; seqNo:2012 2024-11-06T04:49:47Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.51 psi 2024-11-06T04:49:47Z- handleDepth()- Depth_Keller msg: 1730868587.50 epoch sec; seqNo:2154; depth 49.78 2024-11-06T04:49:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868587.52 epoch sec; seqNo:2154; 4061706.74 north; 594652.51 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:49:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868584.29 2024-11-06T04:49:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:47Z- waiting for 1730868588.29, time is 1730868587.52 2024-11-06T04:49:47Z- AHRS_M2 msg: 1730868587.89 epoch sec; seqNo:2013 2024-11-06T04:49:47Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.51 psi 2024-11-06T04:49:47Z- handleDepth()- Depth_Keller msg: 1730868587.90 epoch sec; seqNo:2155; depth 49.75 2024-11-06T04:49:47Z- handleDVL() - DVL msg epochMillisec: 1730868587.90 2024-11-06T04:49:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:47Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:47Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:47Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:47Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:47Z- handleDvl() - RDI_Pathfinder msg: 1730868587.90 sec; seqNo:786; #beams:0 2024-11-06T04:49:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.06 , -0.03 2024-11-06T04:49:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868587.91 epoch sec; seqNo:2155; 4061706.74 north; 594652.51 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:49:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868584.29 2024-11-06T04:49:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:48Z- waiting for 1730868588.29, time is 1730868587.91 2024-11-06T04:49:48Z- AHRS_M2 msg: 1730868588.30 epoch sec; seqNo:2014 2024-11-06T04:49:48Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.51 psi 2024-11-06T04:49:48Z- handleDepth()- Depth_Keller msg: 1730868588.31 epoch sec; seqNo:2156; depth 49.75 2024-11-06T04:49:48Z- handleDVL() - DVL msg epochMillisec: 1730868588.34 2024-11-06T04:49:48Z- handleDvl() - RDI_Pathfinder msg: 1730868588.34 sec; seqNo:787; #beams:4 2024-11-06T04:49:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 31.78 , 36.36 , 34.49 2024-11-06T04:49:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.09 , 0.02 2024-11-06T04:49:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868588.36 epoch sec; seqNo:2156; 4061707.12 north; 594652.51 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:49:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868584.29 2024-11-06T04:49:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:48Z- waiting for 1730868588.29, time is 1730868588.36 2024-11-06T04:49:48Z- LcmTrn::updateTrn() >>>> pose time 1730868588.36, meas time 1730868588.34 <<<< 2024-11-06T04:49:48Z- LcmTrn::updateTrn() equating pose 1730868588.34 and meas times 1730868588.34 2024-11-06T04:49:48Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868588.34 2024-11-06T04:49:48Z- TerrainNav::motionUpdate() - currEstimate = (4061707, 594652.51, 49.75) 2024-11-06T04:49:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:49:48Z- TerrainNav:: SubcloudNIS is 0.23, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:49:48Z- TNavPF::Projecting Measurements 2024-11-06T04:49:48Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:49:48Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:49:48Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:49:48Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:49:48Z- TNavFilter::4 good beams. 2024-11-06T04:49:48Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:49:48Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:49:48Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:49:48Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:49:48Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:49:48Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:49:48Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:49:48Z- TerrainMapOctree: (vehN, vehE ) = (4061707.12, 594652.51). 2024-11-06T04:49:48Z- TerrainMapOctree: Min Distance = 106.50. 2024-11-06T04:49:48Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:49:48Z- Forcing Subcloud Comparison 2024-11-06T04:49:48Z- Weighting particles with subcloud comparison 2024-11-06T04:49:48Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:48Z- meanExpectedMeasDiff: 3.107561 delta_rms_squared: 0.960119 alpha: 0.979594 2024-11-06T04:49:48Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:48Z- meanExpectedMeasDiff: -1.193546 delta_rms_squared: 0.688417 alpha: 0.971768 2024-11-06T04:49:48Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:48Z- meanExpectedMeasDiff: 3.103094 delta_rms_squared: 0.856191 alpha: 0.977174 2024-11-06T04:49:48Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:48Z- meanExpectedMeasDiff: -1.892864 delta_rms_squared: 0.787457 alpha: 0.975231 2024-11-06T04:49:48Z- Subcloud NIS: 0.248775 2024-11-06T04:49:48Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:49:48Z- TNavPF:: sumWeights = 0.042862 2024-11-06T04:49:48Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:49:48Z- TNavPF::First Term of Map Covariance Matrix is 0.33 2024-11-06T04:49:48Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T04:49:48Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868588.34 sec, ping # 0. 2024-11-06T04:49:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:49:48Z- variances([0], [2], and psi_berg) = 0.323329 0.514482 0.000000 2024-11-06T04:49:48Z- publishEstimatesDecoder() - at 1730868588.34 sec 2024-11-06T04:49:48Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:49:48Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061651.44 594656.72 51.67 2024-11-06T04:49:48Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061650.73 594657.04 51.98 lat/lon: 36.70 -121.94 2024-11-06T04:49:48Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.32 0.51 0.32 0.00 2024-11-06T04:49:48Z- AHRS_M2 msg: 1730868588.70 epoch sec; seqNo:2015 2024-11-06T04:49:48Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.51 psi 2024-11-06T04:49:48Z- handleDepth()- Depth_Keller msg: 1730868588.72 epoch sec; seqNo:2157; depth 49.74 2024-11-06T04:49:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868588.74 epoch sec; seqNo:2157; 4061707.12 north; 594652.51 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T04:49:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868588.34 2024-11-06T04:49:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:48Z- waiting for 1730868592.34, time is 1730868588.74 2024-11-06T04:49:49Z- AHRS_M2 msg: 1730868589.11 epoch sec; seqNo:2016 2024-11-06T04:49:49Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.51 psi 2024-11-06T04:49:49Z- handleDepth()- Depth_Keller msg: 1730868589.12 epoch sec; seqNo:2158; depth 49.73 2024-11-06T04:49:49Z- handleDVL() - DVL msg epochMillisec: 1730868589.12 2024-11-06T04:49:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:49Z- handleDvl() - RDI_Pathfinder msg: 1730868589.12 sec; seqNo:788; #beams:0 2024-11-06T04:49:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.09 , 0.02 2024-11-06T04:49:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868589.13 epoch sec; seqNo:2158; 4061707.13 north; 594653.73 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T04:49:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868588.34 2024-11-06T04:49:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:49Z- waiting for 1730868592.34, time is 1730868589.13 2024-11-06T04:49:49Z- AHRS_M2 msg: 1730868589.51 epoch sec; seqNo:2017 2024-11-06T04:49:49Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.51 psi 2024-11-06T04:49:49Z- handleDepth()- Depth_Keller msg: 1730868589.52 epoch sec; seqNo:2159; depth 49.73 2024-11-06T04:49:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868589.53 epoch sec; seqNo:2159; 4061707.13 north; 594653.73 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T04:49:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868588.34 2024-11-06T04:49:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:49Z- waiting for 1730868592.34, time is 1730868589.53 2024-11-06T04:49:49Z- AHRS_M2 msg: 1730868589.92 epoch sec; seqNo:2018 2024-11-06T04:49:49Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.50 psi 2024-11-06T04:49:49Z- handleDepth()- Depth_Keller msg: 1730868589.93 epoch sec; seqNo:2160; depth 49.72 2024-11-06T04:49:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868589.94 epoch sec; seqNo:2160; 4061707.13 north; 594653.73 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:49:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868588.34 2024-11-06T04:49:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:50Z- waiting for 1730868592.34, time is 1730868589.94 2024-11-06T04:49:50Z- AHRS_M2 msg: 1730868590.32 epoch sec; seqNo:2019 2024-11-06T04:49:50Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.50 psi 2024-11-06T04:49:50Z- handleDepth()- Depth_Keller msg: 1730868590.33 epoch sec; seqNo:2161; depth 49.71 2024-11-06T04:49:50Z- handleDVL() - DVL msg epochMillisec: 1730868590.35 2024-11-06T04:49:50Z- handleDvl() - RDI_Pathfinder msg: 1730868590.35 sec; seqNo:789; #beams:4 2024-11-06T04:49:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 31.94 , 36.36 , 34.83 2024-11-06T04:49:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.06 , 0.04 2024-11-06T04:49:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868590.37 epoch sec; seqNo:2161; 4061707.14 north; 594654.95 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T04:49:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868588.34 2024-11-06T04:49:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:50Z- waiting for 1730868592.34, time is 1730868590.37 2024-11-06T04:49:50Z- AHRS_M2 msg: 1730868590.72 epoch sec; seqNo:2020 2024-11-06T04:49:50Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.51 psi 2024-11-06T04:49:50Z- handleDepth()- Depth_Keller msg: 1730868590.73 epoch sec; seqNo:2162; depth 49.69 2024-11-06T04:49:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868590.75 epoch sec; seqNo:2162; 4061707.14 north; 594654.95 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T04:49:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868588.34 2024-11-06T04:49:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:50Z- waiting for 1730868592.34, time is 1730868590.75 2024-11-06T04:49:51Z- AHRS_M2 msg: 1730868591.13 epoch sec; seqNo:2021 2024-11-06T04:49:51Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.51 psi 2024-11-06T04:49:51Z- handleDepth()- Depth_Keller msg: 1730868591.14 epoch sec; seqNo:2163; depth 49.69 2024-11-06T04:49:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868591.15 epoch sec; seqNo:2163; 4061707.52 north; 594654.95 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T04:49:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868588.34 2024-11-06T04:49:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:51Z- waiting for 1730868592.34, time is 1730868591.15 2024-11-06T04:49:51Z- AHRS_M2 msg: 1730868591.53 epoch sec; seqNo:2022 2024-11-06T04:49:51Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.51 psi 2024-11-06T04:49:51Z- handleDepth()- Depth_Keller msg: 1730868591.54 epoch sec; seqNo:2164; depth 49.68 2024-11-06T04:49:51Z- handleDVL() - DVL msg epochMillisec: 1730868591.54 2024-11-06T04:49:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:51Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:51Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:51Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:51Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:51Z- handleDvl() - RDI_Pathfinder msg: 1730868591.54 sec; seqNo:790; #beams:0 2024-11-06T04:49:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.06 , 0.04 2024-11-06T04:49:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868591.56 epoch sec; seqNo:2164; 4061707.52 north; 594654.95 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T04:49:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868588.34 2024-11-06T04:49:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:51Z- waiting for 1730868592.34, time is 1730868591.56 2024-11-06T04:49:51Z- AHRS_M2 msg: 1730868591.94 epoch sec; seqNo:2023 2024-11-06T04:49:51Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.51 psi 2024-11-06T04:49:51Z- handleDepth()- Depth_Keller msg: 1730868591.95 epoch sec; seqNo:2165; depth 49.69 2024-11-06T04:49:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868591.95 epoch sec; seqNo:2165; 4061707.53 north; 594656.17 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T04:49:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868588.34 2024-11-06T04:49:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:52Z- waiting for 1730868592.34, time is 1730868591.95 2024-11-06T04:49:52Z- AHRS_M2 msg: 1730868592.35 epoch sec; seqNo:2024 2024-11-06T04:49:52Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.51 psi 2024-11-06T04:49:52Z- handleDepth()- Depth_Keller msg: 1730868592.35 epoch sec; seqNo:2166; depth 49.67 2024-11-06T04:49:52Z- handleDVL() - DVL msg epochMillisec: 1730868592.37 2024-11-06T04:49:52Z- handleDvl() - RDI_Pathfinder msg: 1730868592.37 sec; seqNo:791; #beams:4 2024-11-06T04:49:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 31.94 , 36.36 , 35.00 2024-11-06T04:49:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.03 2024-11-06T04:49:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868592.40 epoch sec; seqNo:2166; 4061707.53 north; 594656.17 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:49:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868588.34 2024-11-06T04:49:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:52Z- waiting for 1730868592.34, time is 1730868592.40 2024-11-06T04:49:52Z- LcmTrn::updateTrn() >>>> pose time 1730868592.40, meas time 1730868592.37 <<<< 2024-11-06T04:49:52Z- LcmTrn::updateTrn() equating pose 1730868592.37 and meas times 1730868592.37 2024-11-06T04:49:52Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868592.37 2024-11-06T04:49:52Z- TerrainNav::motionUpdate() - currEstimate = (4061708, 594656.17, 49.67) 2024-11-06T04:49:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:49:52Z- TerrainNav:: SubcloudNIS is 0.25, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:49:52Z- TNavPF::Projecting Measurements 2024-11-06T04:49:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:49:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:49:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:49:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:49:52Z- TNavFilter::4 good beams. 2024-11-06T04:49:52Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:49:52Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:49:52Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:49:52Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:49:52Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:49:52Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:49:52Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:49:52Z- TerrainMapOctree: (vehN, vehE ) = (4061707.53, 594656.17). 2024-11-06T04:49:52Z- TerrainMapOctree: Min Distance = 102.99. 2024-11-06T04:49:52Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:49:52Z- Forcing Subcloud Comparison 2024-11-06T04:49:52Z- Weighting particles with subcloud comparison 2024-11-06T04:49:52Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:52Z- meanExpectedMeasDiff: 2.890003 delta_rms_squared: 1.025902 alpha: 0.980878 2024-11-06T04:49:52Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:52Z- meanExpectedMeasDiff: -1.424911 delta_rms_squared: 0.655237 alpha: 0.970381 2024-11-06T04:49:52Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:52Z- meanExpectedMeasDiff: 2.874750 delta_rms_squared: 0.758333 alpha: 0.974304 2024-11-06T04:49:52Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:52Z- meanExpectedMeasDiff: -1.984576 delta_rms_squared: 0.915971 alpha: 0.978632 2024-11-06T04:49:52Z- Subcloud NIS: 0.275429 2024-11-06T04:49:52Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:49:52Z- TNavPF:: sumWeights = 0.047835 2024-11-06T04:49:52Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:49:52Z- TNavPF::First Term of Map Covariance Matrix is 0.34 2024-11-06T04:49:52Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T04:49:52Z- TerrainNav::Resampling particle filter... 2024-11-06T04:49:52Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868592.37 sec, ping # 0. 2024-11-06T04:49:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:49:52Z- variances([0], [2], and psi_berg) = 0.329126 0.530252 0.000000 2024-11-06T04:49:52Z- publishEstimatesDecoder() - at 1730868592.37 sec 2024-11-06T04:49:52Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:49:52Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061651.38 594659.69 52.67 2024-11-06T04:49:52Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061651.15 594660.71 51.72 lat/lon: 36.70 -121.94 2024-11-06T04:49:52Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.53 0.31 0.00 2024-11-06T04:49:52Z- AHRS_M2 msg: 1730868592.74 epoch sec; seqNo:2025 2024-11-06T04:49:52Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.51 psi 2024-11-06T04:49:52Z- handleDepth()- Depth_Keller msg: 1730868592.76 epoch sec; seqNo:2167; depth 49.67 2024-11-06T04:49:52Z- handleDVL() - DVL msg epochMillisec: 1730868592.76 2024-11-06T04:49:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:52Z- handleDvl() - RDI_Pathfinder msg: 1730868592.76 sec; seqNo:792; #beams:0 2024-11-06T04:49:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.03 2024-11-06T04:49:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868592.78 epoch sec; seqNo:2167; 4061707.53 north; 594656.17 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:49:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868592.37 2024-11-06T04:49:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:52Z- waiting for 1730868596.37, time is 1730868592.78 2024-11-06T04:49:53Z- AHRS_M2 msg: 1730868593.16 epoch sec; seqNo:2026 2024-11-06T04:49:53Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.51 psi 2024-11-06T04:49:53Z- handleDepth()- Depth_Keller msg: 1730868593.16 epoch sec; seqNo:2168; depth 49.67 2024-11-06T04:49:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868593.18 epoch sec; seqNo:2168; 4061707.55 north; 594657.39 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:49:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868592.37 2024-11-06T04:49:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:53Z- waiting for 1730868596.37, time is 1730868593.18 2024-11-06T04:49:53Z- AHRS_M2 msg: 1730868593.55 epoch sec; seqNo:2027 2024-11-06T04:49:53Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.51 psi 2024-11-06T04:49:53Z- handleDepth()- Depth_Keller msg: 1730868593.56 epoch sec; seqNo:2169; depth 49.64 2024-11-06T04:49:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868593.57 epoch sec; seqNo:2169; 4061707.55 north; 594657.39 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:49:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868592.37 2024-11-06T04:49:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:53Z- waiting for 1730868596.37, time is 1730868593.57 2024-11-06T04:49:53Z- AHRS_M2 msg: 1730868593.95 epoch sec; seqNo:2028 2024-11-06T04:49:53Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.51 psi 2024-11-06T04:49:53Z- handleDepth()- Depth_Keller msg: 1730868593.98 epoch sec; seqNo:2170; depth 49.64 2024-11-06T04:49:53Z- handleDVL() - DVL msg epochMillisec: 1730868593.98 2024-11-06T04:49:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:53Z- handleDvl() - RDI_Pathfinder msg: 1730868593.98 sec; seqNo:793; #beams:0 2024-11-06T04:49:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.03 2024-11-06T04:49:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868593.99 epoch sec; seqNo:2170; 4061707.55 north; 594657.39 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:49:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868592.37 2024-11-06T04:49:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:54Z- waiting for 1730868596.37, time is 1730868593.99 2024-11-06T04:49:54Z- AHRS_M2 msg: 1730868594.36 epoch sec; seqNo:2029 2024-11-06T04:49:54Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.51 psi 2024-11-06T04:49:54Z- handleDepth()- Depth_Keller msg: 1730868594.37 epoch sec; seqNo:2171; depth 49.64 2024-11-06T04:49:54Z- handleDVL() - DVL msg epochMillisec: 1730868594.40 2024-11-06T04:49:54Z- handleDvl() - RDI_Pathfinder msg: 1730868594.40 sec; seqNo:794; #beams:4 2024-11-06T04:49:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 31.94 , 36.02 , 35.00 2024-11-06T04:49:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.01 2024-11-06T04:49:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868594.41 epoch sec; seqNo:2171; 4061707.55 north; 594657.39 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:49:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868592.37 2024-11-06T04:49:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:54Z- waiting for 1730868596.37, time is 1730868594.41 2024-11-06T04:49:54Z- AHRS_M2 msg: 1730868594.77 epoch sec; seqNo:2030 2024-11-06T04:49:54Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.51 psi 2024-11-06T04:49:54Z- handleDepth()- Depth_Keller msg: 1730868594.77 epoch sec; seqNo:2172; depth 49.62 2024-11-06T04:49:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868594.78 epoch sec; seqNo:2172; 4061707.56 north; 594658.61 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:49:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868592.37 2024-11-06T04:49:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:54Z- waiting for 1730868596.37, time is 1730868594.78 2024-11-06T04:49:55Z- AHRS_M2 msg: 1730868595.17 epoch sec; seqNo:2031 2024-11-06T04:49:55Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.51 psi 2024-11-06T04:49:55Z- handleDepth()- Depth_Keller msg: 1730868595.19 epoch sec; seqNo:2173; depth 49.63 2024-11-06T04:49:55Z- handleDVL() - DVL msg epochMillisec: 1730868595.19 2024-11-06T04:49:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:55Z- handleDvl() - RDI_Pathfinder msg: 1730868595.19 sec; seqNo:795; #beams:0 2024-11-06T04:49:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , -0.01 2024-11-06T04:49:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868595.20 epoch sec; seqNo:2173; 4061707.56 north; 594658.61 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:49:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868592.37 2024-11-06T04:49:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:55Z- waiting for 1730868596.37, time is 1730868595.20 2024-11-06T04:49:55Z- AHRS_M2 msg: 1730868595.57 epoch sec; seqNo:2032 2024-11-06T04:49:55Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.51 psi 2024-11-06T04:49:55Z- handleDepth()- Depth_Keller msg: 1730868595.58 epoch sec; seqNo:2174; depth 49.61 2024-11-06T04:49:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868595.59 epoch sec; seqNo:2174; 4061707.56 north; 594658.61 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:49:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868592.37 2024-11-06T04:49:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:55Z- waiting for 1730868596.37, time is 1730868595.59 2024-11-06T04:49:55Z- AHRS_M2 msg: 1730868595.97 epoch sec; seqNo:2033 2024-11-06T04:49:55Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.51 psi 2024-11-06T04:49:55Z- handleDepth()- Depth_Keller msg: 1730868595.99 epoch sec; seqNo:2175; depth 49.60 2024-11-06T04:49:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868596.00 epoch sec; seqNo:2175; 4061707.58 north; 594659.83 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:49:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868592.37 2024-11-06T04:49:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:56Z- waiting for 1730868596.37, time is 1730868596.00 2024-11-06T04:49:56Z- handleDepth()- Depth_Keller msg: 1730868596.40 epoch sec; seqNo:2176; depth 49.60 2024-11-06T04:49:56Z- handleDVL() - DVL msg epochMillisec: 1730868596.42 2024-11-06T04:49:56Z- handleDvl() - RDI_Pathfinder msg: 1730868596.42 sec; seqNo:796; #beams:4 2024-11-06T04:49:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 31.94 , 36.19 , 34.83 2024-11-06T04:49:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.02 , -0.05 2024-11-06T04:49:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868596.44 epoch sec; seqNo:2176; 4061707.58 north; 594659.83 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:49:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868592.37 2024-11-06T04:49:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:56Z- waiting for 1730868596.37, time is 1730868596.44 2024-11-06T04:49:56Z- LcmTrn::updateTrn() >>>> pose time 1730868596.44, meas time 1730868596.42 <<<< 2024-11-06T04:49:56Z- LcmTrn::updateTrn() equating pose 1730868596.42 and meas times 1730868596.42 2024-11-06T04:49:56Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868596.42 2024-11-06T04:49:56Z- TerrainNav::motionUpdate() - currEstimate = (4061708, 594659.83, 49.60) 2024-11-06T04:49:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:49:56Z- TerrainNav:: SubcloudNIS is 0.28, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:49:56Z- TNavPF::Projecting Measurements 2024-11-06T04:49:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:49:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:49:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:49:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:49:56Z- TNavFilter::4 good beams. 2024-11-06T04:49:56Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:49:56Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:49:56Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:49:56Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:49:56Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:49:56Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:49:56Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:49:56Z- TerrainMapOctree: (vehN, vehE ) = (4061707.58, 594659.83). 2024-11-06T04:49:56Z- TerrainMapOctree: Min Distance = 99.65. 2024-11-06T04:49:56Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:49:56Z- Forcing Subcloud Comparison 2024-11-06T04:49:56Z- Weighting particles with subcloud comparison 2024-11-06T04:49:56Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:56Z- meanExpectedMeasDiff: 2.507204 delta_rms_squared: 0.986938 alpha: 0.980138 2024-11-06T04:49:56Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:56Z- meanExpectedMeasDiff: -1.655721 delta_rms_squared: 0.646915 alpha: 0.970011 2024-11-06T04:49:56Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:56Z- meanExpectedMeasDiff: 1.879131 delta_rms_squared: 1.375166 alpha: 0.985665 2024-11-06T04:49:56Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:49:56Z- meanExpectedMeasDiff: -1.642145 delta_rms_squared: 0.890219 alpha: 0.978027 2024-11-06T04:49:56Z- Subcloud NIS: 0.301825 2024-11-06T04:49:56Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:49:56Z- TNavPF:: sumWeights = 0.112305 2024-11-06T04:49:56Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:49:56Z- TNavPF::First Term of Map Covariance Matrix is 0.57 2024-11-06T04:49:56Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T04:49:56Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868596.42 sec, ping # 0. 2024-11-06T04:49:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:49:56Z- variances([0], [2], and psi_berg) = 0.310287 0.454923 0.000000 2024-11-06T04:49:56Z- publishEstimatesDecoder() - at 1730868596.42 sec 2024-11-06T04:49:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:49:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061653.13 594663.20 51.85 2024-11-06T04:49:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061651.22 594664.50 51.56 lat/lon: 36.70 -121.94 2024-11-06T04:49:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.31 0.45 0.30 0.00 2024-11-06T04:49:56Z- AHRS_M2 msg: 1730868596.78 epoch sec; seqNo:2034 2024-11-06T04:49:56Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.52 psi 2024-11-06T04:49:56Z- handleDepth()- Depth_Keller msg: 1730868596.79 epoch sec; seqNo:2177; depth 49.60 2024-11-06T04:49:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868596.84 epoch sec; seqNo:2177; 4061707.58 north; 594659.83 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:49:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868596.42 2024-11-06T04:49:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:56Z- waiting for 1730868600.42, time is 1730868596.84 2024-11-06T04:49:57Z- AHRS_M2 msg: 1730868597.19 epoch sec; seqNo:2035 2024-11-06T04:49:57Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.51 psi 2024-11-06T04:49:57Z- handleDepth()- Depth_Keller msg: 1730868597.20 epoch sec; seqNo:2178; depth 49.59 2024-11-06T04:49:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868597.21 epoch sec; seqNo:2178; 4061707.58 north; 594659.83 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:49:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868596.42 2024-11-06T04:49:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:57Z- waiting for 1730868600.42, time is 1730868597.21 2024-11-06T04:49:57Z- AHRS_M2 msg: 1730868597.60 epoch sec; seqNo:2036 2024-11-06T04:49:57Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.51 psi 2024-11-06T04:49:57Z- handleDepth()- Depth_Keller msg: 1730868597.61 epoch sec; seqNo:2179; depth 49.59 2024-11-06T04:49:57Z- handleDVL() - DVL msg epochMillisec: 1730868597.61 2024-11-06T04:49:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:57Z- handleDvl() - RDI_Pathfinder msg: 1730868597.61 sec; seqNo:797; #beams:0 2024-11-06T04:49:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.02 , -0.05 2024-11-06T04:49:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868597.62 epoch sec; seqNo:2179; 4061707.59 north; 594661.05 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:49:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868596.42 2024-11-06T04:49:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:57Z- waiting for 1730868600.42, time is 1730868597.62 2024-11-06T04:49:58Z- AHRS_M2 msg: 1730868597.99 epoch sec; seqNo:2037 2024-11-06T04:49:58Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.51 psi 2024-11-06T04:49:58Z- handleDepth()- Depth_Keller msg: 1730868598.01 epoch sec; seqNo:2180; depth 49.59 2024-11-06T04:49:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868598.02 epoch sec; seqNo:2180; 4061707.59 north; 594661.05 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:49:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868596.42 2024-11-06T04:49:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:58Z- waiting for 1730868600.42, time is 1730868598.02 2024-11-06T04:49:58Z- handleDepth()- Depth_Keller msg: 1730868598.42 epoch sec; seqNo:2181; depth 49.60 2024-11-06T04:49:58Z- handleDVL() - DVL msg epochMillisec: 1730868598.44 2024-11-06T04:49:58Z- handleDvl() - RDI_Pathfinder msg: 1730868598.44 sec; seqNo:798; #beams:4 2024-11-06T04:49:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.97 , 31.78 , 36.19 , 34.83 2024-11-06T04:49:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.05 , -0.04 2024-11-06T04:49:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868598.46 epoch sec; seqNo:2181; 4061707.97 north; 594661.04 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:49:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868596.42 2024-11-06T04:49:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:58Z- waiting for 1730868600.42, time is 1730868598.46 2024-11-06T04:49:58Z- AHRS_M2 msg: 1730868598.80 epoch sec; seqNo:2038 2024-11-06T04:49:58Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.51 psi 2024-11-06T04:49:58Z- handleDepth()- Depth_Keller msg: 1730868598.83 epoch sec; seqNo:2182; depth 49.61 2024-11-06T04:49:58Z- handleDVL() - DVL msg epochMillisec: 1730868598.83 2024-11-06T04:49:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:49:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:49:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:49:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:49:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:49:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:49:58Z- handleDvl() - RDI_Pathfinder msg: 1730868598.83 sec; seqNo:799; #beams:0 2024-11-06T04:49:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:49:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.05 , -0.04 2024-11-06T04:49:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868598.85 epoch sec; seqNo:2182; 4061707.98 north; 594662.26 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:49:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:49:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868596.42 2024-11-06T04:49:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:58Z- waiting for 1730868600.42, time is 1730868598.85 2024-11-06T04:49:59Z- AHRS_M2 msg: 1730868599.21 epoch sec; seqNo:2039 2024-11-06T04:49:59Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.51 psi 2024-11-06T04:49:59Z- handleDepth()- Depth_Keller msg: 1730868599.22 epoch sec; seqNo:2183; depth 49.60 2024-11-06T04:49:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868599.23 epoch sec; seqNo:2183; 4061707.98 north; 594662.26 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:49:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868596.42 2024-11-06T04:49:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:59Z- waiting for 1730868600.42, time is 1730868599.23 2024-11-06T04:49:59Z- AHRS_M2 msg: 1730868599.62 epoch sec; seqNo:2040 2024-11-06T04:49:59Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.51 psi 2024-11-06T04:49:59Z- handleDepth()- Depth_Keller msg: 1730868599.62 epoch sec; seqNo:2184; depth 49.62 2024-11-06T04:49:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868599.63 epoch sec; seqNo:2184; 4061707.98 north; 594662.26 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:49:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:49:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868596.42 2024-11-06T04:49:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:49:59Z- waiting for 1730868600.42, time is 1730868599.63 2024-11-06T04:50:00Z- AHRS_M2 msg: 1730868600.01 epoch sec; seqNo:2041 2024-11-06T04:50:00Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.50 psi 2024-11-06T04:50:00Z- handleDepth()- Depth_Keller msg: 1730868600.02 epoch sec; seqNo:2185; depth 49.65 2024-11-06T04:50:00Z- handleDVL() - DVL msg epochMillisec: 1730868600.02 2024-11-06T04:50:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:50:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:50:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:50:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:50:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:50:00Z- handleDvl() - RDI_Pathfinder msg: 1730868600.02 sec; seqNo:800; #beams:0 2024-11-06T04:50:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:50:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.05 , -0.04 2024-11-06T04:50:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868600.04 epoch sec; seqNo:2185; 4061707.98 north; 594662.26 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:50:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868596.42 2024-11-06T04:50:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:00Z- waiting for 1730868600.42, time is 1730868600.04 2024-11-06T04:50:00Z- AHRS_M2 msg: 1730868600.42 epoch sec; seqNo:2042 2024-11-06T04:50:00Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.50 psi 2024-11-06T04:50:00Z- handleDepth()- Depth_Keller msg: 1730868600.44 epoch sec; seqNo:2186; depth 49.64 2024-11-06T04:50:00Z- handleDVL() - DVL msg epochMillisec: 1730868600.46 2024-11-06T04:50:00Z- handleDvl() - RDI_Pathfinder msg: 1730868600.46 sec; seqNo:801; #beams:4 2024-11-06T04:50:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 31.78 , 35.85 , 35.00 2024-11-06T04:50:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.09 , 0.01 2024-11-06T04:50:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868600.48 epoch sec; seqNo:2186; 4061707.99 north; 594663.48 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:50:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868596.42 2024-11-06T04:50:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:00Z- waiting for 1730868600.42, time is 1730868600.48 2024-11-06T04:50:00Z- LcmTrn::updateTrn() >>>> pose time 1730868600.48, meas time 1730868600.46 <<<< 2024-11-06T04:50:00Z- LcmTrn::updateTrn() equating pose 1730868600.46 and meas times 1730868600.46 2024-11-06T04:50:00Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868600.46 2024-11-06T04:50:00Z- TerrainNav::motionUpdate() - currEstimate = (4061708, 594663.48, 49.64) 2024-11-06T04:50:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:50:00Z- TerrainNav:: SubcloudNIS is 0.30, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:50:00Z- TNavPF::Projecting Measurements 2024-11-06T04:50:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:50:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:50:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:50:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:50:00Z- TNavFilter::4 good beams. 2024-11-06T04:50:00Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:50:00Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:50:00Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:50:00Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:50:00Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:50:00Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:50:00Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:50:00Z- TerrainMapOctree: (vehN, vehE ) = (4061707.99, 594663.48). 2024-11-06T04:50:00Z- TerrainMapOctree: Min Distance = 96.17. 2024-11-06T04:50:00Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:50:00Z- Forcing Subcloud Comparison 2024-11-06T04:50:00Z- Weighting particles with subcloud comparison 2024-11-06T04:50:00Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:00Z- meanExpectedMeasDiff: 1.611370 delta_rms_squared: 2.253308 alpha: 0.991202 2024-11-06T04:50:00Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:00Z- meanExpectedMeasDiff: -1.096841 delta_rms_squared: 0.909615 alpha: 0.978486 2024-11-06T04:50:00Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:00Z- meanExpectedMeasDiff: 1.059879 delta_rms_squared: 1.221986 alpha: 0.983897 2024-11-06T04:50:00Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:00Z- meanExpectedMeasDiff: -1.692825 delta_rms_squared: 0.893664 alpha: 0.978110 2024-11-06T04:50:00Z- Subcloud NIS: 0.314953 2024-11-06T04:50:00Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:50:00Z- TNavPF:: sumWeights = 0.238105 2024-11-06T04:50:00Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:50:00Z- TNavPF::First Term of Map Covariance Matrix is 0.59 2024-11-06T04:50:00Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T04:50:00Z- TerrainNav::Resampling particle filter... 2024-11-06T04:50:00Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868600.46 sec, ping # 0. 2024-11-06T04:50:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:50:00Z- variances([0], [2], and psi_berg) = 0.311576 0.438914 0.000000 2024-11-06T04:50:00Z- publishEstimatesDecoder() - at 1730868600.46 sec 2024-11-06T04:50:00Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:50:00Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061652.11 594667.35 51.56 2024-11-06T04:50:00Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061651.70 594668.28 51.57 lat/lon: 36.70 -121.94 2024-11-06T04:50:00Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.31 0.44 0.27 0.00 2024-11-06T04:50:00Z- AHRS_M2 msg: 1730868600.83 epoch sec; seqNo:2043 2024-11-06T04:50:00Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.50 psi 2024-11-06T04:50:00Z- handleDepth()- Depth_Keller msg: 1730868600.84 epoch sec; seqNo:2187; depth 49.64 2024-11-06T04:50:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868600.86 epoch sec; seqNo:2187; 4061707.99 north; 594663.48 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:50:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868600.46 2024-11-06T04:50:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:00Z- waiting for 1730868604.46, time is 1730868600.86 2024-11-06T04:50:01Z- AHRS_M2 msg: 1730868601.23 epoch sec; seqNo:2044 2024-11-06T04:50:01Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.50 psi 2024-11-06T04:50:01Z- handleDepth()- Depth_Keller msg: 1730868601.23 epoch sec; seqNo:2188; depth 49.67 2024-11-06T04:50:01Z- handleDVL() - DVL msg epochMillisec: 1730868601.24 2024-11-06T04:50:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:50:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:50:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:50:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:50:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:50:01Z- handleDvl() - RDI_Pathfinder msg: 1730868601.24 sec; seqNo:802; #beams:0 2024-11-06T04:50:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:50:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.09 , 0.01 2024-11-06T04:50:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868601.26 epoch sec; seqNo:2188; 4061707.99 north; 594663.48 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:50:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868600.46 2024-11-06T04:50:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:01Z- waiting for 1730868604.46, time is 1730868601.26 2024-11-06T04:50:01Z- AHRS_M2 msg: 1730868601.66 epoch sec; seqNo:2045 2024-11-06T04:50:01Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.50 psi 2024-11-06T04:50:01Z- handleDepth()- Depth_Keller msg: 1730868601.69 epoch sec; seqNo:2189; depth 49.68 2024-11-06T04:50:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868601.77 epoch sec; seqNo:2189; 4061708.37 north; 594663.48 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T04:50:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868600.46 2024-11-06T04:50:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:01Z- waiting for 1730868604.46, time is 1730868601.77 2024-11-06T04:50:02Z- AHRS_M2 msg: 1730868602.04 epoch sec; seqNo:2046 2024-11-06T04:50:02Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.50 psi 2024-11-06T04:50:02Z- handleDepth()- Depth_Keller msg: 1730868602.04 epoch sec; seqNo:2190; depth 49.68 2024-11-06T04:50:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868602.05 epoch sec; seqNo:2190; 4061708.39 north; 594664.70 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T04:50:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868600.46 2024-11-06T04:50:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:02Z- waiting for 1730868604.46, time is 1730868602.05 2024-11-06T04:50:02Z- AHRS_M2 msg: 1730868602.45 epoch sec; seqNo:2047 2024-11-06T04:50:02Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:02Z- handleDepth()- Depth_Keller msg: 1730868602.45 epoch sec; seqNo:2191; depth 49.71 2024-11-06T04:50:02Z- handleDVL() - DVL msg epochMillisec: 1730868602.48 2024-11-06T04:50:02Z- handleDvl() - RDI_Pathfinder msg: 1730868602.48 sec; seqNo:803; #beams:4 2024-11-06T04:50:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 31.77 , 35.85 , 35.17 2024-11-06T04:50:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.08 , 0.05 2024-11-06T04:50:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868602.50 epoch sec; seqNo:2191; 4061708.39 north; 594664.70 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T04:50:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868600.46 2024-11-06T04:50:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:02Z- waiting for 1730868604.46, time is 1730868602.50 2024-11-06T04:50:02Z- AHRS_M2 msg: 1730868602.85 epoch sec; seqNo:2048 2024-11-06T04:50:02Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:02Z- handleDepth()- Depth_Keller msg: 1730868602.86 epoch sec; seqNo:2192; depth 49.72 2024-11-06T04:50:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868602.88 epoch sec; seqNo:2192; 4061708.39 north; 594664.70 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:50:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868600.46 2024-11-06T04:50:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:03Z- waiting for 1730868604.46, time is 1730868602.88 2024-11-06T04:50:03Z- AHRS_M2 msg: 1730868603.26 epoch sec; seqNo:2049 2024-11-06T04:50:03Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:03Z- handleDepth()- Depth_Keller msg: 1730868603.26 epoch sec; seqNo:2193; depth 49.74 2024-11-06T04:50:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868603.28 epoch sec; seqNo:2193; 4061708.39 north; 594664.70 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T04:50:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868600.46 2024-11-06T04:50:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:03Z- waiting for 1730868604.46, time is 1730868603.28 2024-11-06T04:50:03Z- AHRS_M2 msg: 1730868603.66 epoch sec; seqNo:2050 2024-11-06T04:50:03Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:03Z- handleDepth()- Depth_Keller msg: 1730868603.66 epoch sec; seqNo:2194; depth 49.76 2024-11-06T04:50:03Z- handleDVL() - DVL msg epochMillisec: 1730868603.67 2024-11-06T04:50:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:50:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:50:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:50:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:50:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:50:03Z- handleDvl() - RDI_Pathfinder msg: 1730868603.67 sec; seqNo:804; #beams:0 2024-11-06T04:50:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:50:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.08 , 0.05 2024-11-06T04:50:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868603.68 epoch sec; seqNo:2194; 4061708.40 north; 594665.92 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T04:50:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868600.46 2024-11-06T04:50:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:03Z- waiting for 1730868604.46, time is 1730868603.68 2024-11-06T04:50:04Z- AHRS_M2 msg: 1730868604.06 epoch sec; seqNo:2051 2024-11-06T04:50:04Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:04Z- handleDepth()- Depth_Keller msg: 1730868604.07 epoch sec; seqNo:2195; depth 49.77 2024-11-06T04:50:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868604.08 epoch sec; seqNo:2195; 4061708.40 north; 594665.92 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T04:50:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868600.46 2024-11-06T04:50:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:04Z- waiting for 1730868604.46, time is 1730868604.08 2024-11-06T04:50:04Z- AHRS_M2 msg: 1730868604.46 epoch sec; seqNo:2052 2024-11-06T04:50:04Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:04Z- handleDepth()- Depth_Keller msg: 1730868604.47 epoch sec; seqNo:2196; depth 49.79 2024-11-06T04:50:04Z- handleDVL() - DVL msg epochMillisec: 1730868604.49 2024-11-06T04:50:04Z- handleDvl() - RDI_Pathfinder msg: 1730868604.49 sec; seqNo:805; #beams:4 2024-11-06T04:50:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 31.78 , 36.02 , 35.00 2024-11-06T04:50:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.05 2024-11-06T04:50:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868604.51 epoch sec; seqNo:2196; 4061708.78 north; 594665.92 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:50:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868600.46 2024-11-06T04:50:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:04Z- waiting for 1730868604.46, time is 1730868604.51 2024-11-06T04:50:04Z- LcmTrn::updateTrn() >>>> pose time 1730868604.51, meas time 1730868604.49 <<<< 2024-11-06T04:50:04Z- LcmTrn::updateTrn() equating pose 1730868604.49 and meas times 1730868604.49 2024-11-06T04:50:04Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868604.49 2024-11-06T04:50:04Z- TerrainNav::motionUpdate() - currEstimate = (4061709, 594665.92, 49.79) 2024-11-06T04:50:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:50:04Z- TerrainNav:: SubcloudNIS is 0.31, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:50:04Z- TNavPF::Projecting Measurements 2024-11-06T04:50:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:50:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:50:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:50:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:50:04Z- TNavFilter::4 good beams. 2024-11-06T04:50:04Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:50:04Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:50:04Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:50:04Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:50:04Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:50:04Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:50:04Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:50:04Z- TerrainMapOctree: (vehN, vehE ) = (4061708.78, 594665.92). 2024-11-06T04:50:04Z- TerrainMapOctree: Min Distance = 93.64. 2024-11-06T04:50:04Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:50:04Z- Forcing Subcloud Comparison 2024-11-06T04:50:04Z- Weighting particles with subcloud comparison 2024-11-06T04:50:04Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:04Z- meanExpectedMeasDiff: 1.239414 delta_rms_squared: 1.487865 alpha: 0.986736 2024-11-06T04:50:04Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:04Z- meanExpectedMeasDiff: -0.759795 delta_rms_squared: 0.605252 alpha: 0.968013 2024-11-06T04:50:04Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:04Z- meanExpectedMeasDiff: 0.860274 delta_rms_squared: 0.830110 alpha: 0.976474 2024-11-06T04:50:04Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:04Z- meanExpectedMeasDiff: -0.675239 delta_rms_squared: 1.000421 alpha: 0.980400 2024-11-06T04:50:04Z- Subcloud NIS: 0.319169 2024-11-06T04:50:04Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:50:04Z- TNavPF:: sumWeights = 0.473062 2024-11-06T04:50:04Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:50:04Z- TNavPF::First Term of Map Covariance Matrix is 0.57 2024-11-06T04:50:04Z- TNavPF::Calculated NIS Value : 0.26 numBeams normalized NIS: 0.07 2024-11-06T04:50:04Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868604.49 sec, ping # 0. 2024-11-06T04:50:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:50:04Z- variances([0], [2], and psi_berg) = 0.295905 0.378833 0.000000 2024-11-06T04:50:04Z- publishEstimatesDecoder() - at 1730868604.49 sec 2024-11-06T04:50:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:50:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061653.70 594670.25 51.85 2024-11-06T04:50:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061652.52 594670.72 51.68 lat/lon: 36.70 -121.94 2024-11-06T04:50:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.30 0.38 0.28 0.00 2024-11-06T04:50:04Z- AHRS_M2 msg: 1730868604.86 epoch sec; seqNo:2053 2024-11-06T04:50:04Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:04Z- handleDepth()- Depth_Keller msg: 1730868604.88 epoch sec; seqNo:2197; depth 49.80 2024-11-06T04:50:04Z- handleDVL() - DVL msg epochMillisec: 1730868604.88 2024-11-06T04:50:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:50:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:50:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:50:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:50:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:50:04Z- handleDvl() - RDI_Pathfinder msg: 1730868604.88 sec; seqNo:806; #beams:0 2024-11-06T04:50:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:50:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.05 2024-11-06T04:50:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868604.90 epoch sec; seqNo:2197; 4061708.79 north; 594667.14 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T04:50:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868604.49 2024-11-06T04:50:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:05Z- waiting for 1730868608.49, time is 1730868604.90 2024-11-06T04:50:05Z- AHRS_M2 msg: 1730868605.27 epoch sec; seqNo:2054 2024-11-06T04:50:05Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:05Z- handleDepth()- Depth_Keller msg: 1730868605.28 epoch sec; seqNo:2198; depth 49.81 2024-11-06T04:50:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868605.29 epoch sec; seqNo:2198; 4061708.79 north; 594667.14 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:50:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868604.49 2024-11-06T04:50:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:05Z- waiting for 1730868608.49, time is 1730868605.29 2024-11-06T04:50:05Z- AHRS_M2 msg: 1730868605.67 epoch sec; seqNo:2055 2024-11-06T04:50:05Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:05Z- handleDepth()- Depth_Keller msg: 1730868605.69 epoch sec; seqNo:2199; depth 49.82 2024-11-06T04:50:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868605.70 epoch sec; seqNo:2199; 4061708.79 north; 594667.14 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:50:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868604.49 2024-11-06T04:50:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:05Z- waiting for 1730868608.49, time is 1730868605.70 2024-11-06T04:50:06Z- AHRS_M2 msg: 1730868606.08 epoch sec; seqNo:2056 2024-11-06T04:50:06Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:06Z- handleDepth()- Depth_Keller msg: 1730868606.09 epoch sec; seqNo:2200; depth 49.84 2024-11-06T04:50:06Z- handleDVL() - DVL msg epochMillisec: 1730868606.10 2024-11-06T04:50:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:50:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:50:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:50:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:50:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:50:06Z- handleDvl() - RDI_Pathfinder msg: 1730868606.10 sec; seqNo:807; #beams:0 2024-11-06T04:50:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:50:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.05 2024-11-06T04:50:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868606.14 epoch sec; seqNo:2200; 4061708.79 north; 594667.14 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T04:50:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868604.49 2024-11-06T04:50:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:06Z- waiting for 1730868608.49, time is 1730868606.14 2024-11-06T04:50:06Z- AHRS_M2 msg: 1730868606.48 epoch sec; seqNo:2057 2024-11-06T04:50:06Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:06Z- handleDepth()- Depth_Keller msg: 1730868606.49 epoch sec; seqNo:2201; depth 49.83 2024-11-06T04:50:06Z- handleDVL() - DVL msg epochMillisec: 1730868606.51 2024-11-06T04:50:06Z- handleDvl() - RDI_Pathfinder msg: 1730868606.51 sec; seqNo:808; #beams:4 2024-11-06T04:50:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 31.78 , 36.19 , 34.49 2024-11-06T04:50:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , 0.00 2024-11-06T04:50:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868606.54 epoch sec; seqNo:2201; 4061708.81 north; 594668.36 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:50:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868604.49 2024-11-06T04:50:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:06Z- waiting for 1730868608.49, time is 1730868606.54 2024-11-06T04:50:06Z- AHRS_M2 msg: 1730868606.88 epoch sec; seqNo:2058 2024-11-06T04:50:06Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:06Z- handleDepth()- Depth_Keller msg: 1730868606.89 epoch sec; seqNo:2202; depth 49.85 2024-11-06T04:50:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868606.90 epoch sec; seqNo:2202; 4061708.81 north; 594668.36 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:50:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868604.49 2024-11-06T04:50:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:07Z- waiting for 1730868608.49, time is 1730868606.90 2024-11-06T04:50:07Z- AHRS_M2 msg: 1730868607.29 epoch sec; seqNo:2059 2024-11-06T04:50:07Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:07Z- handleDepth()- Depth_Keller msg: 1730868607.31 epoch sec; seqNo:2203; depth 49.85 2024-11-06T04:50:07Z- handleDVL() - DVL msg epochMillisec: 1730868607.31 2024-11-06T04:50:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:50:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:50:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:50:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:50:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:50:07Z- handleDvl() - RDI_Pathfinder msg: 1730868607.31 sec; seqNo:809; #beams:0 2024-11-06T04:50:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:50:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , 0.00 2024-11-06T04:50:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868607.32 epoch sec; seqNo:2203; 4061708.81 north; 594668.36 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:50:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868604.49 2024-11-06T04:50:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:07Z- waiting for 1730868608.49, time is 1730868607.32 2024-11-06T04:50:07Z- AHRS_M2 msg: 1730868607.69 epoch sec; seqNo:2060 2024-11-06T04:50:07Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:07Z- handleDepth()- Depth_Keller msg: 1730868607.70 epoch sec; seqNo:2204; depth 49.86 2024-11-06T04:50:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868607.71 epoch sec; seqNo:2204; 4061708.82 north; 594669.58 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:50:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868604.49 2024-11-06T04:50:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:07Z- waiting for 1730868608.49, time is 1730868607.71 2024-11-06T04:50:08Z- AHRS_M2 msg: 1730868608.13 epoch sec; seqNo:2061 2024-11-06T04:50:08Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:08Z- handleDepth()- Depth_Keller msg: 1730868608.15 epoch sec; seqNo:2205; depth 49.85 2024-11-06T04:50:08Z- handleDVL() - DVL msg epochMillisec: 1730868608.17 2024-11-06T04:50:08Z- handleDvl() - RDI_Pathfinder msg: 1730868608.17 sec; seqNo:810; #beams:4 2024-11-06T04:50:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.80 , 31.44 , 36.02 , 34.49 2024-11-06T04:50:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , -0.05 2024-11-06T04:50:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868608.20 epoch sec; seqNo:2205; 4061708.82 north; 594669.58 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:50:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868604.49 2024-11-06T04:50:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:08Z- waiting for 1730868608.49, time is 1730868608.20 2024-11-06T04:50:08Z- AHRS_M2 msg: 1730868608.50 epoch sec; seqNo:2062 2024-11-06T04:50:08Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:08Z- handleDepth()- Depth_Keller msg: 1730868608.51 epoch sec; seqNo:2206; depth 49.85 2024-11-06T04:50:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868608.53 epoch sec; seqNo:2206; 4061708.82 north; 594669.58 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:50:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868604.49 2024-11-06T04:50:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:08Z- waiting for 1730868608.49, time is 1730868608.53 2024-11-06T04:50:08Z- LcmTrn::updateTrn() >>>> pose time 1730868608.53, meas time 1730868608.17 <<<< 2024-11-06T04:50:08Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868608.53 2024-11-06T04:50:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868608.17 sec, ping # 0; waiting for INS data more recent than 1730868604.49... 2024-11-06T04:50:08Z- TerrainNav::motionUpdate() - currEstimate = (4061709, 594669.58, 49.85) 2024-11-06T04:50:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:50:08Z- TerrainNav:: SubcloudNIS is 0.32, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:50:08Z- TNavPF::Projecting Measurements 2024-11-06T04:50:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:50:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:50:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:50:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:50:08Z- TNavFilter::4 good beams. 2024-11-06T04:50:08Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:50:08Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:50:08Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:50:08Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:50:08Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:50:08Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:50:08Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:50:08Z- TerrainMapOctree: (vehN, vehE ) = (4061708.82, 594669.26). 2024-11-06T04:50:08Z- TerrainMapOctree: Min Distance = 90.64. 2024-11-06T04:50:08Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:50:08Z- Forcing Subcloud Comparison 2024-11-06T04:50:08Z- Weighting particles with subcloud comparison 2024-11-06T04:50:08Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:08Z- meanExpectedMeasDiff: -0.319288 delta_rms_squared: 2.577207 alpha: 0.992299 2024-11-06T04:50:08Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:08Z- meanExpectedMeasDiff: -0.947716 delta_rms_squared: 0.612809 alpha: 0.968395 2024-11-06T04:50:08Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:08Z- meanExpectedMeasDiff: 1.595710 delta_rms_squared: 0.804728 alpha: 0.975750 2024-11-06T04:50:08Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:08Z- meanExpectedMeasDiff: -2.282987 delta_rms_squared: 0.733229 alpha: 0.973448 2024-11-06T04:50:08Z- Subcloud NIS: 0.337740 2024-11-06T04:50:08Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:50:08Z- TNavPF:: sumWeights = 0.218469 2024-11-06T04:50:08Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:50:08Z- TNavPF::First Term of Map Covariance Matrix is 0.33 2024-11-06T04:50:08Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T04:50:08Z- TerrainNav::Resampling particle filter... 2024-11-06T04:50:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868608.17 sec. 2024-11-06T04:50:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:50:08Z- variances([0], [2], and psi_berg) = 0.334706 0.408730 0.000000 2024-11-06T04:50:08Z- publishEstimatesDecoder() - at 1730868608.53 sec 2024-11-06T04:50:08Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:50:08Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061653.02 594673.48 51.53 2024-11-06T04:50:08Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061652.66 594674.32 51.92 lat/lon: 36.70 -121.94 2024-11-06T04:50:08Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.41 0.55 0.00 2024-11-06T04:50:08Z- AHRS_M2 msg: 1730868608.91 epoch sec; seqNo:2063 2024-11-06T04:50:08Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:08Z- handleDepth()- Depth_Keller msg: 1730868608.92 epoch sec; seqNo:2207; depth 49.84 2024-11-06T04:50:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868608.95 epoch sec; seqNo:2207; 4061708.83 north; 594670.80 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T04:50:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868608.53 2024-11-06T04:50:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:09Z- waiting for 1730868612.53, time is 1730868608.95 2024-11-06T04:50:09Z- AHRS_M2 msg: 1730868609.31 epoch sec; seqNo:2064 2024-11-06T04:50:09Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:09Z- handleDepth()- Depth_Keller msg: 1730868609.31 epoch sec; seqNo:2208; depth 49.85 2024-11-06T04:50:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868609.32 epoch sec; seqNo:2208; 4061709.21 north; 594670.79 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:50:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868608.53 2024-11-06T04:50:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:09Z- waiting for 1730868612.53, time is 1730868609.32 2024-11-06T04:50:09Z- AHRS_M2 msg: 1730868609.71 epoch sec; seqNo:2065 2024-11-06T04:50:09Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:09Z- handleDepth()- Depth_Keller msg: 1730868609.72 epoch sec; seqNo:2209; depth 49.87 2024-11-06T04:50:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868609.73 epoch sec; seqNo:2209; 4061709.21 north; 594670.79 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:50:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868608.53 2024-11-06T04:50:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:09Z- waiting for 1730868612.53, time is 1730868609.73 2024-11-06T04:50:10Z- AHRS_M2 msg: 1730868610.15 epoch sec; seqNo:2066 2024-11-06T04:50:10Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:10Z- handleDepth()- Depth_Keller msg: 1730868610.16 epoch sec; seqNo:2210; depth 49.86 2024-11-06T04:50:10Z- handleDVL() - DVL msg epochMillisec: 1730868610.18 2024-11-06T04:50:10Z- handleDvl() - RDI_Pathfinder msg: 1730868610.18 sec; seqNo:811; #beams:4 2024-11-06T04:50:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 31.27 , 35.68 , 34.32 2024-11-06T04:50:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.02 , -0.07 2024-11-06T04:50:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868610.20 epoch sec; seqNo:2210; 4061709.21 north; 594670.79 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:50:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868608.53 2024-11-06T04:50:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:10Z- waiting for 1730868612.53, time is 1730868610.20 2024-11-06T04:50:10Z- AHRS_M2 msg: 1730868610.52 epoch sec; seqNo:2067 2024-11-06T04:50:10Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:10Z- handleDepth()- Depth_Keller msg: 1730868610.53 epoch sec; seqNo:2211; depth 49.85 2024-11-06T04:50:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868610.55 epoch sec; seqNo:2211; 4061709.23 north; 594672.01 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:50:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868608.53 2024-11-06T04:50:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:10Z- waiting for 1730868612.53, time is 1730868610.55 2024-11-06T04:50:10Z- AHRS_M2 msg: 1730868610.92 epoch sec; seqNo:2068 2024-11-06T04:50:10Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:10Z- handleDepth()- Depth_Keller msg: 1730868610.93 epoch sec; seqNo:2212; depth 49.86 2024-11-06T04:50:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868610.94 epoch sec; seqNo:2212; 4061709.23 north; 594672.01 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:50:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868608.53 2024-11-06T04:50:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:11Z- waiting for 1730868612.53, time is 1730868610.94 2024-11-06T04:50:11Z- AHRS_M2 msg: 1730868611.33 epoch sec; seqNo:2069 2024-11-06T04:50:11Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:11Z- handleDepth()- Depth_Keller msg: 1730868611.35 epoch sec; seqNo:2213; depth 49.87 2024-11-06T04:50:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868611.36 epoch sec; seqNo:2213; 4061709.23 north; 594672.01 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:50:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868608.53 2024-11-06T04:50:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:11Z- waiting for 1730868612.53, time is 1730868611.36 2024-11-06T04:50:11Z- AHRS_M2 msg: 1730868611.73 epoch sec; seqNo:2070 2024-11-06T04:50:11Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:11Z- handleDepth()- Depth_Keller msg: 1730868611.74 epoch sec; seqNo:2214; depth 49.88 2024-11-06T04:50:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868611.75 epoch sec; seqNo:2214; 4061709.23 north; 594672.01 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T04:50:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868608.53 2024-11-06T04:50:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:11Z- waiting for 1730868612.53, time is 1730868611.75 2024-11-06T04:50:12Z- AHRS_M2 msg: 1730868612.15 epoch sec; seqNo:2071 2024-11-06T04:50:12Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:12Z- handleDepth()- Depth_Keller msg: 1730868612.16 epoch sec; seqNo:2215; depth 49.87 2024-11-06T04:50:12Z- handleDVL() - DVL msg epochMillisec: 1730868612.19 2024-11-06T04:50:12Z- handleDvl() - RDI_Pathfinder msg: 1730868612.19 sec; seqNo:812; #beams:4 2024-11-06T04:50:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.12 , 30.93 , 35.68 , 32.79 2024-11-06T04:50:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.05 , -0.04 2024-11-06T04:50:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868612.20 epoch sec; seqNo:2215; 4061709.24 north; 594673.24 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:50:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868608.53 2024-11-06T04:50:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:12Z- waiting for 1730868612.53, time is 1730868612.20 2024-11-06T04:50:12Z- AHRS_M2 msg: 1730868612.54 epoch sec; seqNo:2072 2024-11-06T04:50:12Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.49 psi 2024-11-06T04:50:12Z- handleDepth()- Depth_Keller msg: 1730868612.55 epoch sec; seqNo:2216; depth 49.89 2024-11-06T04:50:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868612.56 epoch sec; seqNo:2216; 4061709.24 north; 594673.24 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:50:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868608.53 2024-11-06T04:50:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:12Z- waiting for 1730868612.53, time is 1730868612.56 2024-11-06T04:50:12Z- LcmTrn::updateTrn() >>>> pose time 1730868612.56, meas time 1730868612.19 <<<< 2024-11-06T04:50:12Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868612.56 2024-11-06T04:50:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868612.19 sec, ping # 0; waiting for INS data more recent than 1730868608.53... 2024-11-06T04:50:12Z- TerrainNav::motionUpdate() - currEstimate = (4061709, 594673.24, 49.89) 2024-11-06T04:50:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:50:12Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:50:12Z- TNavPF::Projecting Measurements 2024-11-06T04:50:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:50:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:50:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:50:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:50:12Z- TNavFilter::4 good beams. 2024-11-06T04:50:12Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:50:12Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:50:12Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:50:12Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:50:12Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:50:12Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:50:12Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:50:12Z- TerrainMapOctree: (vehN, vehE ) = (4061709.20, 594672.89). 2024-11-06T04:50:12Z- TerrainMapOctree: Min Distance = 87.24. 2024-11-06T04:50:12Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:50:12Z- Forcing Subcloud Comparison 2024-11-06T04:50:12Z- Weighting particles with subcloud comparison 2024-11-06T04:50:12Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:12Z- meanExpectedMeasDiff: -2.066698 delta_rms_squared: 2.574093 alpha: 0.992290 2024-11-06T04:50:12Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:12Z- meanExpectedMeasDiff: -1.195548 delta_rms_squared: 0.918746 alpha: 0.978695 2024-11-06T04:50:12Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:12Z- meanExpectedMeasDiff: 0.892814 delta_rms_squared: 2.024594 alpha: 0.990218 2024-11-06T04:50:12Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:12Z- meanExpectedMeasDiff: -3.460076 delta_rms_squared: 1.848703 alpha: 0.989297 2024-11-06T04:50:12Z- Subcloud NIS: 0.370377 2024-11-06T04:50:12Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:50:12Z- TNavPF:: sumWeights = 0.104167 2024-11-06T04:50:12Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:50:12Z- TNavPF::First Term of Map Covariance Matrix is 0.18 2024-11-06T04:50:12Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T04:50:12Z- TerrainNav::Resampling particle filter... 2024-11-06T04:50:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868612.19 sec. 2024-11-06T04:50:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:50:12Z- variances([0], [2], and psi_berg) = 0.326269 0.381950 0.000000 2024-11-06T04:50:12Z- publishEstimatesDecoder() - at 1730868612.56 sec 2024-11-06T04:50:12Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:50:12Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061653.38 594677.03 53.42 2024-11-06T04:50:12Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061653.20 594678.14 52.64 lat/lon: 36.70 -121.94 2024-11-06T04:50:12Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.38 0.55 0.00 2024-11-06T04:50:12Z- AHRS_M2 msg: 1730868612.94 epoch sec; seqNo:2073 2024-11-06T04:50:12Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:50:12Z- handleDepth()- Depth_Keller msg: 1730868612.96 epoch sec; seqNo:2217; depth 49.89 2024-11-06T04:50:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868612.97 epoch sec; seqNo:2217; 4061709.24 north; 594673.24 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:50:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868612.56 2024-11-06T04:50:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:13Z- waiting for 1730868616.56, time is 1730868612.97 2024-11-06T04:50:13Z- AHRS_M2 msg: 1730868613.35 epoch sec; seqNo:2074 2024-11-06T04:50:13Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:50:13Z- handleDepth()- Depth_Keller msg: 1730868613.35 epoch sec; seqNo:2218; depth 49.92 2024-11-06T04:50:13Z- handleDVL() - DVL msg epochMillisec: 1730868613.36 2024-11-06T04:50:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:50:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:50:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:50:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:50:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:50:13Z- handleDvl() - RDI_Pathfinder msg: 1730868613.36 sec; seqNo:813; #beams:0 2024-11-06T04:50:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:50:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.05 , -0.04 2024-11-06T04:50:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868613.36 epoch sec; seqNo:2218; 4061709.24 north; 594673.24 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:50:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868612.56 2024-11-06T04:50:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:13Z- waiting for 1730868616.56, time is 1730868613.36 2024-11-06T04:50:13Z- AHRS_M2 msg: 1730868613.75 epoch sec; seqNo:2075 2024-11-06T04:50:13Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:50:13Z- handleDepth()- Depth_Keller msg: 1730868613.76 epoch sec; seqNo:2219; depth 49.92 2024-11-06T04:50:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868613.77 epoch sec; seqNo:2219; 4061709.63 north; 594674.45 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:50:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868612.56 2024-11-06T04:50:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:13Z- waiting for 1730868616.56, time is 1730868613.77 2024-11-06T04:50:14Z- AHRS_M2 msg: 1730868614.16 epoch sec; seqNo:2076 2024-11-06T04:50:14Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:50:14Z- handleDepth()- Depth_Keller msg: 1730868614.17 epoch sec; seqNo:2220; depth 49.94 2024-11-06T04:50:14Z- handleDVL() - DVL msg epochMillisec: 1730868614.19 2024-11-06T04:50:14Z- handleDvl() - RDI_Pathfinder msg: 1730868614.19 sec; seqNo:814; #beams:4 2024-11-06T04:50:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.12 , 30.93 , 36.02 , 32.79 2024-11-06T04:50:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.06 , 0.02 2024-11-06T04:50:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868614.20 epoch sec; seqNo:2220; 4061709.63 north; 594674.45 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T04:50:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868612.56 2024-11-06T04:50:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:14Z- waiting for 1730868616.56, time is 1730868614.20 2024-11-06T04:50:14Z- AHRS_M2 msg: 1730868614.56 epoch sec; seqNo:2077 2024-11-06T04:50:14Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:50:14Z- handleDepth()- Depth_Keller msg: 1730868614.56 epoch sec; seqNo:2221; depth 49.95 2024-11-06T04:50:14Z- handleDVL() - DVL msg epochMillisec: 1730868614.57 2024-11-06T04:50:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:50:14Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:50:14Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:50:14Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:50:14Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:50:14Z- handleDvl() - RDI_Pathfinder msg: 1730868614.57 sec; seqNo:815; #beams:0 2024-11-06T04:50:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:50:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.06 , 0.02 2024-11-06T04:50:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868614.58 epoch sec; seqNo:2221; 4061709.63 north; 594674.45 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T04:50:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868612.56 2024-11-06T04:50:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:14Z- waiting for 1730868616.56, time is 1730868614.58 2024-11-06T04:50:14Z- AHRS_M2 msg: 1730868614.96 epoch sec; seqNo:2078 2024-11-06T04:50:14Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:50:14Z- handleDepth()- Depth_Keller msg: 1730868614.97 epoch sec; seqNo:2222; depth 49.97 2024-11-06T04:50:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868614.98 epoch sec; seqNo:2222; 4061709.63 north; 594674.45 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T04:50:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868612.56 2024-11-06T04:50:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:15Z- waiting for 1730868616.56, time is 1730868614.98 2024-11-06T04:50:15Z- AHRS_M2 msg: 1730868615.37 epoch sec; seqNo:2079 2024-11-06T04:50:15Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:50:15Z- handleDepth()- Depth_Keller msg: 1730868615.38 epoch sec; seqNo:2223; depth 49.98 2024-11-06T04:50:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868615.39 epoch sec; seqNo:2223; 4061709.64 north; 594675.67 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T04:50:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868612.56 2024-11-06T04:50:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:15Z- waiting for 1730868616.56, time is 1730868615.39 2024-11-06T04:50:15Z- AHRS_M2 msg: 1730868615.77 epoch sec; seqNo:2080 2024-11-06T04:50:15Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:50:15Z- handleDepth()- Depth_Keller msg: 1730868615.78 epoch sec; seqNo:2224; depth 50.00 2024-11-06T04:50:15Z- handleDVL() - DVL msg epochMillisec: 1730868615.79 2024-11-06T04:50:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:50:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:50:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:50:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:50:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:50:15Z- handleDvl() - RDI_Pathfinder msg: 1730868615.79 sec; seqNo:816; #beams:0 2024-11-06T04:50:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:50:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.06 , 0.02 2024-11-06T04:50:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868615.84 epoch sec; seqNo:2224; 4061709.64 north; 594675.67 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T04:50:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868612.56 2024-11-06T04:50:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:15Z- waiting for 1730868616.56, time is 1730868615.84 2024-11-06T04:50:16Z- AHRS_M2 msg: 1730868616.17 epoch sec; seqNo:2081 2024-11-06T04:50:16Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 1.48 psi 2024-11-06T04:50:16Z- handleDepth()- Depth_Keller msg: 1730868616.18 epoch sec; seqNo:2225; depth 50.03 2024-11-06T04:50:16Z- handleDVL() - DVL msg epochMillisec: 1730868616.20 2024-11-06T04:50:16Z- handleDvl() - RDI_Pathfinder msg: 1730868616.20 sec; seqNo:817; #beams:4 2024-11-06T04:50:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.12 , 30.76 , 36.02 , 32.97 2024-11-06T04:50:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.05 2024-11-06T04:50:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868616.23 epoch sec; seqNo:2225; 4061709.64 north; 594675.67 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T04:50:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868612.56 2024-11-06T04:50:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:16Z- waiting for 1730868616.56, time is 1730868616.23 2024-11-06T04:50:16Z- handleDepth()- Depth_Keller msg: 1730868616.60 epoch sec; seqNo:2226; depth 50.05 2024-11-06T04:50:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868616.61 epoch sec; seqNo:2226; 4061710.04 north; 594676.89 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T04:50:16Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:50:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868612.56 2024-11-06T04:50:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:16Z- waiting for 1730868616.56, time is 1730868616.61 2024-11-06T04:50:16Z- LcmTrn::updateTrn() >>>> pose time 1730868616.61, meas time 1730868616.20 <<<< 2024-11-06T04:50:16Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868616.61 2024-11-06T04:50:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868616.20 sec, ping # 0; waiting for INS data more recent than 1730868612.56... 2024-11-06T04:50:16Z- TerrainNav::motionUpdate() - currEstimate = (4061710, 594676.89, 50.05) 2024-11-06T04:50:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:50:16Z- TerrainNav:: SubcloudNIS is 0.37, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:50:16Z- TNavPF::Projecting Measurements 2024-11-06T04:50:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:50:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:50:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:50:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:50:16Z- TNavFilter::4 good beams. 2024-11-06T04:50:16Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:50:16Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:50:16Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:50:16Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:50:16Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:50:16Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:50:16Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:50:16Z- TerrainMapOctree: (vehN, vehE ) = (4061709.96, 594676.52). 2024-11-06T04:50:16Z- TerrainMapOctree: Min Distance = 83.68. 2024-11-06T04:50:16Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:50:16Z- Forcing Subcloud Comparison 2024-11-06T04:50:16Z- Weighting particles with subcloud comparison 2024-11-06T04:50:16Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:16Z- meanExpectedMeasDiff: -1.983218 delta_rms_squared: 2.000966 alpha: 0.990104 2024-11-06T04:50:16Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:16Z- meanExpectedMeasDiff: -0.453501 delta_rms_squared: 0.915559 alpha: 0.978622 2024-11-06T04:50:16Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:16Z- meanExpectedMeasDiff: -0.056998 delta_rms_squared: 1.947171 alpha: 0.989833 2024-11-06T04:50:16Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:16Z- meanExpectedMeasDiff: -1.420881 delta_rms_squared: 1.174058 alpha: 0.983250 2024-11-06T04:50:16Z- Subcloud NIS: 0.357107 2024-11-06T04:50:16Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:50:16Z- TNavPF:: sumWeights = 0.386567 2024-11-06T04:50:16Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:50:16Z- TNavPF::First Term of Map Covariance Matrix is 0.52 2024-11-06T04:50:16Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T04:50:16Z- TerrainNav::Resampling particle filter... 2024-11-06T04:50:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868616.20 sec. 2024-11-06T04:50:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:50:16Z- variances([0], [2], and psi_berg) = 0.314399 0.388202 0.000000 2024-11-06T04:50:16Z- publishEstimatesDecoder() - at 1730868616.61 sec 2024-11-06T04:50:16Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:50:16Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061654.13 594680.66 53.81 2024-11-06T04:50:16Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061654.02 594681.82 53.22 lat/lon: 36.70 -121.94 2024-11-06T04:50:16Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.31 0.39 0.58 0.00 2024-11-06T04:50:17Z- AHRS_M2 msg: 1730868616.99 epoch sec; seqNo:2082 2024-11-06T04:50:17Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:50:17Z- handleDepth()- Depth_Keller msg: 1730868617.01 epoch sec; seqNo:2227; depth 50.07 2024-11-06T04:50:17Z- handleDVL() - DVL msg epochMillisec: 1730868617.01 2024-11-06T04:50:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:50:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:50:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:50:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:50:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:50:17Z- handleDvl() - RDI_Pathfinder msg: 1730868617.01 sec; seqNo:818; #beams:0 2024-11-06T04:50:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:50:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.05 2024-11-06T04:50:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868617.03 epoch sec; seqNo:2227; 4061710.04 north; 594676.89 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T04:50:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868616.61 2024-11-06T04:50:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:17Z- waiting for 1730868620.61, time is 1730868617.03 2024-11-06T04:50:17Z- AHRS_M2 msg: 1730868617.39 epoch sec; seqNo:2083 2024-11-06T04:50:17Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:50:17Z- handleDepth()- Depth_Keller msg: 1730868617.39 epoch sec; seqNo:2228; depth 50.08 2024-11-06T04:50:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868617.40 epoch sec; seqNo:2228; 4061710.04 north; 594676.89 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T04:50:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868616.61 2024-11-06T04:50:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:17Z- waiting for 1730868620.61, time is 1730868617.40 2024-11-06T04:50:17Z- AHRS_M2 msg: 1730868617.80 epoch sec; seqNo:2084 2024-11-06T04:50:17Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:50:17Z- handleDepth()- Depth_Keller msg: 1730868617.80 epoch sec; seqNo:2229; depth 50.10 2024-11-06T04:50:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868617.82 epoch sec; seqNo:2229; 4061710.04 north; 594676.89 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T04:50:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868616.61 2024-11-06T04:50:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:17Z- waiting for 1730868620.61, time is 1730868617.82 2024-11-06T04:50:18Z- AHRS_M2 msg: 1730868618.19 epoch sec; seqNo:2085 2024-11-06T04:50:18Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:50:18Z- handleDepth()- Depth_Keller msg: 1730868618.20 epoch sec; seqNo:2230; depth 50.12 2024-11-06T04:50:18Z- handleDVL() - DVL msg epochMillisec: 1730868618.23 2024-11-06T04:50:18Z- handleDvl() - RDI_Pathfinder msg: 1730868618.23 sec; seqNo:819; #beams:4 2024-11-06T04:50:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 30.75 , 36.36 , 33.47 2024-11-06T04:50:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.04 2024-11-06T04:50:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868618.27 epoch sec; seqNo:2230; 4061710.05 north; 594678.11 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T04:50:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868616.61 2024-11-06T04:50:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:18Z- waiting for 1730868620.61, time is 1730868618.27 2024-11-06T04:50:18Z- AHRS_M2 msg: 1730868618.60 epoch sec; seqNo:2086 2024-11-06T04:50:18Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:50:18Z- handleDepth()- Depth_Keller msg: 1730868618.60 epoch sec; seqNo:2231; depth 50.14 2024-11-06T04:50:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868618.62 epoch sec; seqNo:2231; 4061710.05 north; 594678.11 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:50:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868616.61 2024-11-06T04:50:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:18Z- waiting for 1730868620.61, time is 1730868618.62 2024-11-06T04:50:19Z- AHRS_M2 msg: 1730868619.00 epoch sec; seqNo:2087 2024-11-06T04:50:19Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:50:19Z- handleDepth()- Depth_Keller msg: 1730868619.01 epoch sec; seqNo:2232; depth 50.14 2024-11-06T04:50:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868619.02 epoch sec; seqNo:2232; 4061710.05 north; 594678.11 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:50:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868616.61 2024-11-06T04:50:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:19Z- waiting for 1730868620.61, time is 1730868619.02 2024-11-06T04:50:19Z- AHRS_M2 msg: 1730868619.41 epoch sec; seqNo:2088 2024-11-06T04:50:19Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:50:19Z- handleDepth()- Depth_Keller msg: 1730868619.41 epoch sec; seqNo:2233; depth 50.16 2024-11-06T04:50:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868619.42 epoch sec; seqNo:2233; 4061710.06 north; 594679.33 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T04:50:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868616.61 2024-11-06T04:50:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:19Z- waiting for 1730868620.61, time is 1730868619.42 2024-11-06T04:50:19Z- AHRS_M2 msg: 1730868619.83 epoch sec; seqNo:2089 2024-11-06T04:50:19Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:50:19Z- handleDepth()- Depth_Keller msg: 1730868619.84 epoch sec; seqNo:2234; depth 50.19 2024-11-06T04:50:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868619.85 epoch sec; seqNo:2234; 4061710.44 north; 594679.32 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:50:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868616.61 2024-11-06T04:50:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:19Z- waiting for 1730868620.61, time is 1730868619.85 2024-11-06T04:50:20Z- handleDepth()- Depth_Keller msg: 1730868620.23 epoch sec; seqNo:2235; depth 50.18 2024-11-06T04:50:20Z- handleDVL() - DVL msg epochMillisec: 1730868620.25 2024-11-06T04:50:20Z- handleDvl() - RDI_Pathfinder msg: 1730868620.25 sec; seqNo:820; #beams:4 2024-11-06T04:50:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 30.59 , 36.70 , 33.65 2024-11-06T04:50:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.00 , 0.00 2024-11-06T04:50:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868620.27 epoch sec; seqNo:2235; 4061710.44 north; 594679.32 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T04:50:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868616.61 2024-11-06T04:50:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:20Z- waiting for 1730868620.61, time is 1730868620.27 2024-11-06T04:50:20Z- AHRS_M2 msg: 1730868620.63 epoch sec; seqNo:2090 2024-11-06T04:50:20Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.48 psi 2024-11-06T04:50:20Z- handleDepth()- Depth_Keller msg: 1730868620.63 epoch sec; seqNo:2236; depth 50.20 2024-11-06T04:50:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868620.64 epoch sec; seqNo:2236; 4061710.44 north; 594679.32 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:50:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868616.61 2024-11-06T04:50:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:20Z- waiting for 1730868620.61, time is 1730868620.64 2024-11-06T04:50:20Z- LcmTrn::updateTrn() >>>> pose time 1730868620.64, meas time 1730868620.25 <<<< 2024-11-06T04:50:20Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868620.64 2024-11-06T04:50:20Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868620.25 sec, ping # 0; waiting for INS data more recent than 1730868616.61... 2024-11-06T04:50:20Z- TerrainNav::motionUpdate() - currEstimate = (4061710, 594679.32, 50.20) 2024-11-06T04:50:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:50:20Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:50:20Z- TNavPF::Projecting Measurements 2024-11-06T04:50:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:50:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:50:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:50:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:50:20Z- TNavFilter::4 good beams. 2024-11-06T04:50:20Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:50:20Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:50:20Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:50:20Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:50:20Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:50:20Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:50:20Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:50:20Z- TerrainMapOctree: (vehN, vehE ) = (4061710.40, 594679.09). 2024-11-06T04:50:20Z- TerrainMapOctree: Min Distance = 81.22. 2024-11-06T04:50:20Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:50:20Z- Forcing Subcloud Comparison 2024-11-06T04:50:20Z- Weighting particles with subcloud comparison 2024-11-06T04:50:20Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:20Z- meanExpectedMeasDiff: -2.159319 delta_rms_squared: 2.349046 alpha: 0.991558 2024-11-06T04:50:20Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:20Z- meanExpectedMeasDiff: 0.901198 delta_rms_squared: 1.554624 alpha: 0.987299 2024-11-06T04:50:20Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:20Z- meanExpectedMeasDiff: -0.143492 delta_rms_squared: 1.657776 alpha: 0.988079 2024-11-06T04:50:20Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:20Z- meanExpectedMeasDiff: 1.398351 delta_rms_squared: 1.707019 alpha: 0.988419 2024-11-06T04:50:20Z- Subcloud NIS: 0.358524 2024-11-06T04:50:20Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:50:20Z- TNavPF:: sumWeights = 0.220699 2024-11-06T04:50:20Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:50:20Z- TNavPF::First Term of Map Covariance Matrix is 0.89 2024-11-06T04:50:20Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.03 2024-11-06T04:50:20Z- TerrainNav::Resampling particle filter... 2024-11-06T04:50:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868620.25 sec. 2024-11-06T04:50:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:50:20Z- variances([0], [2], and psi_berg) = 0.333802 0.412423 0.000000 2024-11-06T04:50:20Z- publishEstimatesDecoder() - at 1730868620.64 sec 2024-11-06T04:50:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:50:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061654.58 594683.10 53.52 2024-11-06T04:50:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061654.32 594684.21 53.37 lat/lon: 36.70 -121.94 2024-11-06T04:50:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.33 0.41 0.59 0.00 2024-11-06T04:50:21Z- AHRS_M2 msg: 1730868621.04 epoch sec; seqNo:2091 2024-11-06T04:50:21Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:21Z- handleDepth()- Depth_Keller msg: 1730868621.04 epoch sec; seqNo:2237; depth 50.22 2024-11-06T04:50:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868621.07 epoch sec; seqNo:2237; 4061710.46 north; 594680.54 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T04:50:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868620.64 2024-11-06T04:50:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:21Z- waiting for 1730868624.64, time is 1730868621.07 2024-11-06T04:50:21Z- AHRS_M2 msg: 1730868621.43 epoch sec; seqNo:2092 2024-11-06T04:50:21Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:21Z- handleDepth()- Depth_Keller msg: 1730868621.45 epoch sec; seqNo:2238; depth 50.21 2024-11-06T04:50:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868621.46 epoch sec; seqNo:2238; 4061710.46 north; 594680.54 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T04:50:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868620.64 2024-11-06T04:50:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:21Z- waiting for 1730868624.64, time is 1730868621.46 2024-11-06T04:50:21Z- AHRS_M2 msg: 1730868621.83 epoch sec; seqNo:2093 2024-11-06T04:50:21Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:21Z- handleDepth()- Depth_Keller msg: 1730868621.84 epoch sec; seqNo:2239; depth 50.21 2024-11-06T04:50:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868621.85 epoch sec; seqNo:2239; 4061710.46 north; 594680.54 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T04:50:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868620.64 2024-11-06T04:50:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:22Z- waiting for 1730868624.64, time is 1730868621.85 2024-11-06T04:50:22Z- AHRS_M2 msg: 1730868622.23 epoch sec; seqNo:2094 2024-11-06T04:50:22Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.49 psi 2024-11-06T04:50:22Z- handleDepth()- Depth_Keller msg: 1730868622.25 epoch sec; seqNo:2240; depth 50.21 2024-11-06T04:50:22Z- handleDVL() - DVL msg epochMillisec: 1730868622.27 2024-11-06T04:50:22Z- handleDvl() - RDI_Pathfinder msg: 1730868622.27 sec; seqNo:821; #beams:4 2024-11-06T04:50:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.80 , 29.91 , 37.21 , 33.81 2024-11-06T04:50:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , -0.04 2024-11-06T04:50:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868622.29 epoch sec; seqNo:2240; 4061710.47 north; 594681.76 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T04:50:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868620.64 2024-11-06T04:50:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:22Z- waiting for 1730868624.64, time is 1730868622.29 2024-11-06T04:50:22Z- AHRS_M2 msg: 1730868622.65 epoch sec; seqNo:2095 2024-11-06T04:50:22Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:22Z- handleDepth()- Depth_Keller msg: 1730868622.65 epoch sec; seqNo:2241; depth 50.21 2024-11-06T04:50:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868622.67 epoch sec; seqNo:2241; 4061710.47 north; 594681.76 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T04:50:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868620.64 2024-11-06T04:50:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:22Z- waiting for 1730868624.64, time is 1730868622.67 2024-11-06T04:50:23Z- handleDepth()- Depth_Keller msg: 1730868623.09 epoch sec; seqNo:2242; depth 50.20 2024-11-06T04:50:23Z- handleDVL() - DVL msg epochMillisec: 1730868623.12 2024-11-06T04:50:23Z- handleDvl() - RDI_Pathfinder msg: 1730868623.12 sec; seqNo:822; #beams:4 2024-11-06T04:50:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.97 , 29.56 , 37.38 , 33.98 2024-11-06T04:50:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , -0.05 2024-11-06T04:50:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868623.13 epoch sec; seqNo:2242; 4061710.47 north; 594681.76 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:50:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868620.64 2024-11-06T04:50:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:23Z- waiting for 1730868624.64, time is 1730868623.13 2024-11-06T04:50:23Z- AHRS_M2 msg: 1730868623.45 epoch sec; seqNo:2096 2024-11-06T04:50:23Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:23Z- handleDepth()- Depth_Keller msg: 1730868623.46 epoch sec; seqNo:2243; depth 50.21 2024-11-06T04:50:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868623.49 epoch sec; seqNo:2243; 4061710.48 north; 594682.99 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T04:50:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868620.64 2024-11-06T04:50:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:23Z- waiting for 1730868624.64, time is 1730868623.49 2024-11-06T04:50:23Z- AHRS_M2 msg: 1730868623.85 epoch sec; seqNo:2097 2024-11-06T04:50:23Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:23Z- handleDepth()- Depth_Keller msg: 1730868623.86 epoch sec; seqNo:2244; depth 50.19 2024-11-06T04:50:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868623.88 epoch sec; seqNo:2244; 4061710.48 north; 594682.99 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:50:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868620.64 2024-11-06T04:50:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:24Z- waiting for 1730868624.64, time is 1730868623.88 2024-11-06T04:50:24Z- AHRS_M2 msg: 1730868624.26 epoch sec; seqNo:2098 2024-11-06T04:50:24Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:24Z- handleDepth()- Depth_Keller msg: 1730868624.27 epoch sec; seqNo:2245; depth 50.19 2024-11-06T04:50:24Z- handleDVL() - DVL msg epochMillisec: 1730868624.29 2024-11-06T04:50:24Z- handleDvl() - RDI_Pathfinder msg: 1730868624.29 sec; seqNo:823; #beams:4 2024-11-06T04:50:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.14 , 29.56 , 37.21 , 33.81 2024-11-06T04:50:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.01 , -0.05 2024-11-06T04:50:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868624.30 epoch sec; seqNo:2245; 4061710.48 north; 594682.99 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:50:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868620.64 2024-11-06T04:50:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:24Z- waiting for 1730868624.64, time is 1730868624.30 2024-11-06T04:50:24Z- AHRS_M2 msg: 1730868624.66 epoch sec; seqNo:2099 2024-11-06T04:50:24Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.49 psi 2024-11-06T04:50:24Z- handleDepth()- Depth_Keller msg: 1730868624.67 epoch sec; seqNo:2246; depth 50.20 2024-11-06T04:50:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868624.69 epoch sec; seqNo:2246; 4061710.86 north; 594682.98 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:50:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868620.64 2024-11-06T04:50:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:24Z- waiting for 1730868624.64, time is 1730868624.69 2024-11-06T04:50:24Z- LcmTrn::updateTrn() >>>> pose time 1730868624.69, meas time 1730868624.29 <<<< 2024-11-06T04:50:24Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868624.69 2024-11-06T04:50:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868624.29 sec, ping # 0; waiting for INS data more recent than 1730868620.64... 2024-11-06T04:50:24Z- TerrainNav::motionUpdate() - currEstimate = (4061711, 594682.98, 50.20) 2024-11-06T04:50:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:50:24Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:50:24Z- TNavPF::Projecting Measurements 2024-11-06T04:50:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:50:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:50:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:50:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:50:24Z- TNavFilter::4 good beams. 2024-11-06T04:50:24Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:50:24Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:50:24Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:50:24Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:50:24Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:50:24Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:50:24Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:50:24Z- TerrainMapOctree: (vehN, vehE ) = (4061710.82, 594682.61). 2024-11-06T04:50:24Z- TerrainMapOctree: Min Distance = 77.95. 2024-11-06T04:50:24Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:50:24Z- Forcing Subcloud Comparison 2024-11-06T04:50:24Z- Weighting particles with subcloud comparison 2024-11-06T04:50:24Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:24Z- meanExpectedMeasDiff: -1.915868 delta_rms_squared: 1.964634 alpha: 0.989923 2024-11-06T04:50:24Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:24Z- meanExpectedMeasDiff: 0.317306 delta_rms_squared: 0.960355 alpha: 0.979599 2024-11-06T04:50:24Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:24Z- meanExpectedMeasDiff: -0.023353 delta_rms_squared: 1.120661 alpha: 0.982466 2024-11-06T04:50:24Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:24Z- meanExpectedMeasDiff: 2.567098 delta_rms_squared: 1.197822 alpha: 0.983577 2024-11-06T04:50:24Z- Subcloud NIS: 0.370304 2024-11-06T04:50:24Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:50:24Z- TNavPF:: sumWeights = 0.175775 2024-11-06T04:50:24Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:50:24Z- TNavPF::First Term of Map Covariance Matrix is 0.70 2024-11-06T04:50:24Z- TNavPF::Calculated NIS Value : 0.09 numBeams normalized NIS: 0.02 2024-11-06T04:50:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868624.29 sec. 2024-11-06T04:50:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:50:24Z- variances([0], [2], and psi_berg) = 0.377360 0.426752 0.000000 2024-11-06T04:50:24Z- publishEstimatesDecoder() - at 1730868624.69 sec 2024-11-06T04:50:24Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:50:24Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061653.33 594686.80 52.73 2024-11-06T04:50:24Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061654.70 594687.87 53.25 lat/lon: 36.70 -121.94 2024-11-06T04:50:24Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.38 0.43 0.63 0.00 2024-11-06T04:50:25Z- AHRS_M2 msg: 1730868625.11 epoch sec; seqNo:2100 2024-11-06T04:50:25Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.49 psi 2024-11-06T04:50:25Z- handleDepth()- Depth_Keller msg: 1730868625.11 epoch sec; seqNo:2247; depth 50.19 2024-11-06T04:50:25Z- handleDVL() - DVL msg epochMillisec: 1730868625.16 2024-11-06T04:50:25Z- handleDvl() - RDI_Pathfinder msg: 1730868625.16 sec; seqNo:824; #beams:4 2024-11-06T04:50:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 29.57 , 37.04 , 33.81 2024-11-06T04:50:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.02 , -0.04 2024-11-06T04:50:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868625.18 epoch sec; seqNo:2247; 4061710.88 north; 594684.20 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:50:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868624.69 2024-11-06T04:50:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:25Z- waiting for 1730868628.69, time is 1730868625.18 2024-11-06T04:50:25Z- AHRS_M2 msg: 1730868625.47 epoch sec; seqNo:2101 2024-11-06T04:50:25Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.49 psi 2024-11-06T04:50:25Z- handleDepth()- Depth_Keller msg: 1730868625.48 epoch sec; seqNo:2248; depth 50.19 2024-11-06T04:50:25Z- handleDVL() - DVL msg epochMillisec: 1730868625.48 2024-11-06T04:50:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:50:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:50:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:50:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:50:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:50:25Z- handleDvl() - RDI_Pathfinder msg: 1730868625.48 sec; seqNo:825; #beams:0 2024-11-06T04:50:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:50:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.02 , -0.04 2024-11-06T04:50:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868625.49 epoch sec; seqNo:2248; 4061710.88 north; 594684.20 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:50:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868624.69 2024-11-06T04:50:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:25Z- waiting for 1730868628.69, time is 1730868625.49 2024-11-06T04:50:25Z- AHRS_M2 msg: 1730868625.88 epoch sec; seqNo:2102 2024-11-06T04:50:25Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.49 psi 2024-11-06T04:50:25Z- handleDepth()- Depth_Keller msg: 1730868625.88 epoch sec; seqNo:2249; depth 50.19 2024-11-06T04:50:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868625.89 epoch sec; seqNo:2249; 4061710.88 north; 594684.20 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:50:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868624.69 2024-11-06T04:50:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:26Z- waiting for 1730868628.69, time is 1730868625.89 2024-11-06T04:50:26Z- AHRS_M2 msg: 1730868626.28 epoch sec; seqNo:2103 2024-11-06T04:50:26Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.49 psi 2024-11-06T04:50:26Z- handleDepth()- Depth_Keller msg: 1730868626.28 epoch sec; seqNo:2250; depth 50.19 2024-11-06T04:50:26Z- handleDVL() - DVL msg epochMillisec: 1730868626.31 2024-11-06T04:50:26Z- handleDvl() - RDI_Pathfinder msg: 1730868626.31 sec; seqNo:826; #beams:4 2024-11-06T04:50:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.80 , 29.57 , 37.04 , 33.65 2024-11-06T04:50:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.02 , -0.02 2024-11-06T04:50:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868626.32 epoch sec; seqNo:2250; 4061710.88 north; 594684.20 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:50:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868624.69 2024-11-06T04:50:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:26Z- waiting for 1730868628.69, time is 1730868626.32 2024-11-06T04:50:26Z- AHRS_M2 msg: 1730868626.68 epoch sec; seqNo:2104 2024-11-06T04:50:26Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.48 psi 2024-11-06T04:50:26Z- handleDepth()- Depth_Keller msg: 1730868626.69 epoch sec; seqNo:2251; depth 50.20 2024-11-06T04:50:26Z- handleDVL() - DVL msg epochMillisec: 1730868626.69 2024-11-06T04:50:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:50:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:50:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:50:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:50:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:50:26Z- handleDvl() - RDI_Pathfinder msg: 1730868626.69 sec; seqNo:827; #beams:0 2024-11-06T04:50:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:50:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.02 , -0.02 2024-11-06T04:50:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868626.71 epoch sec; seqNo:2251; 4061710.89 north; 594685.42 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:50:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868624.69 2024-11-06T04:50:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:26Z- waiting for 1730868628.69, time is 1730868626.71 2024-11-06T04:50:27Z- AHRS_M2 msg: 1730868627.12 epoch sec; seqNo:2105 2024-11-06T04:50:27Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.49 psi 2024-11-06T04:50:27Z- handleDepth()- Depth_Keller msg: 1730868627.12 epoch sec; seqNo:2252; depth 50.21 2024-11-06T04:50:27Z- handleDVL() - DVL msg epochMillisec: 1730868627.15 2024-11-06T04:50:27Z- handleDvl() - RDI_Pathfinder msg: 1730868627.15 sec; seqNo:828; #beams:4 2024-11-06T04:50:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 29.23 , 37.04 , 33.99 2024-11-06T04:50:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.04 , -0.00 2024-11-06T04:50:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868627.16 epoch sec; seqNo:2252; 4061710.89 north; 594685.42 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T04:50:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868624.69 2024-11-06T04:50:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:27Z- waiting for 1730868628.69, time is 1730868627.16 2024-11-06T04:50:27Z- AHRS_M2 msg: 1730868627.49 epoch sec; seqNo:2106 2024-11-06T04:50:27Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.49 psi 2024-11-06T04:50:27Z- handleDepth()- Depth_Keller msg: 1730868627.50 epoch sec; seqNo:2253; depth 50.19 2024-11-06T04:50:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868627.52 epoch sec; seqNo:2253; 4061710.89 north; 594685.42 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:50:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868624.69 2024-11-06T04:50:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:27Z- waiting for 1730868628.69, time is 1730868627.52 2024-11-06T04:50:27Z- AHRS_M2 msg: 1730868627.89 epoch sec; seqNo:2107 2024-11-06T04:50:27Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.49 psi 2024-11-06T04:50:27Z- handleDepth()- Depth_Keller msg: 1730868627.90 epoch sec; seqNo:2254; depth 50.20 2024-11-06T04:50:27Z- handleDVL() - DVL msg epochMillisec: 1730868627.90 2024-11-06T04:50:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:50:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:50:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:50:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:50:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:50:27Z- handleDvl() - RDI_Pathfinder msg: 1730868627.90 sec; seqNo:829; #beams:0 2024-11-06T04:50:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:50:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.04 , -0.00 2024-11-06T04:50:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868627.93 epoch sec; seqNo:2254; 4061710.89 north; 594685.42 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:50:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868624.69 2024-11-06T04:50:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:28Z- waiting for 1730868628.69, time is 1730868627.93 2024-11-06T04:50:28Z- AHRS_M2 msg: 1730868628.30 epoch sec; seqNo:2108 2024-11-06T04:50:28Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.48 psi 2024-11-06T04:50:28Z- handleDepth()- Depth_Keller msg: 1730868628.31 epoch sec; seqNo:2255; depth 50.21 2024-11-06T04:50:28Z- handleDVL() - DVL msg epochMillisec: 1730868628.33 2024-11-06T04:50:28Z- handleDvl() - RDI_Pathfinder msg: 1730868628.33 sec; seqNo:830; #beams:4 2024-11-06T04:50:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 29.23 , 36.87 , 33.81 2024-11-06T04:50:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.05 , 0.01 2024-11-06T04:50:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868628.36 epoch sec; seqNo:2255; 4061710.90 north; 594686.64 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T04:50:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868624.69 2024-11-06T04:50:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:28Z- waiting for 1730868628.69, time is 1730868628.36 2024-11-06T04:50:28Z- AHRS_M2 msg: 1730868628.71 epoch sec; seqNo:2109 2024-11-06T04:50:28Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.48 psi 2024-11-06T04:50:28Z- handleDepth()- Depth_Keller msg: 1730868628.71 epoch sec; seqNo:2256; depth 50.22 2024-11-06T04:50:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868628.72 epoch sec; seqNo:2256; 4061711.28 north; 594686.64 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T04:50:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868624.69 2024-11-06T04:50:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:28Z- waiting for 1730868628.69, time is 1730868628.72 2024-11-06T04:50:28Z- LcmTrn::updateTrn() >>>> pose time 1730868628.72, meas time 1730868628.33 <<<< 2024-11-06T04:50:28Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868628.72 2024-11-06T04:50:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868628.33 sec, ping # 0; waiting for INS data more recent than 1730868624.69... 2024-11-06T04:50:28Z- TerrainNav::motionUpdate() - currEstimate = (4061711, 594686.64, 50.22) 2024-11-06T04:50:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:50:28Z- TerrainNav:: SubcloudNIS is 0.37, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:50:28Z- TNavPF::Projecting Measurements 2024-11-06T04:50:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:50:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:50:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:50:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:50:28Z- TNavFilter::4 good beams. 2024-11-06T04:50:28Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:50:28Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:50:28Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:50:28Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:50:28Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:50:28Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:50:28Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:50:28Z- TerrainMapOctree: (vehN, vehE ) = (4061711.24, 594686.28). 2024-11-06T04:50:28Z- TerrainMapOctree: Min Distance = 74.58. 2024-11-06T04:50:28Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:50:28Z- Forcing Subcloud Comparison 2024-11-06T04:50:28Z- Weighting particles with subcloud comparison 2024-11-06T04:50:28Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:28Z- meanExpectedMeasDiff: -2.613612 delta_rms_squared: 2.184049 alpha: 0.990926 2024-11-06T04:50:28Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:28Z- meanExpectedMeasDiff: -0.316890 delta_rms_squared: 1.022050 alpha: 0.980807 2024-11-06T04:50:28Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:28Z- meanExpectedMeasDiff: -0.522026 delta_rms_squared: 1.151663 alpha: 0.982930 2024-11-06T04:50:28Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:29Z- meanExpectedMeasDiff: 1.578080 delta_rms_squared: 0.935203 alpha: 0.979062 2024-11-06T04:50:29Z- Subcloud NIS: 0.384440 2024-11-06T04:50:29Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:50:29Z- TNavPF:: sumWeights = 0.222721 2024-11-06T04:50:29Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:50:29Z- TNavPF::First Term of Map Covariance Matrix is 0.91 2024-11-06T04:50:29Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T04:50:29Z- TerrainNav::Resampling particle filter... 2024-11-06T04:50:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868628.33 sec. 2024-11-06T04:50:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:50:29Z- variances([0], [2], and psi_berg) = 0.411923 0.407449 0.000000 2024-11-06T04:50:29Z- publishEstimatesDecoder() - at 1730868628.72 sec 2024-11-06T04:50:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:50:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061655.32 594690.41 53.23 2024-11-06T04:50:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061655.07 594691.61 53.56 lat/lon: 36.70 -121.94 2024-11-06T04:50:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.41 0.41 0.65 0.00 2024-11-06T04:50:29Z- AHRS_M2 msg: 1730868629.14 epoch sec; seqNo:2110 2024-11-06T04:50:29Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.48 psi 2024-11-06T04:50:29Z- handleDepth()- Depth_Keller msg: 1730868629.15 epoch sec; seqNo:2257; depth 50.24 2024-11-06T04:50:29Z- handleDVL() - DVL msg epochMillisec: 1730868629.19 2024-11-06T04:50:29Z- handleDvl() - RDI_Pathfinder msg: 1730868629.19 sec; seqNo:831; #beams:4 2024-11-06T04:50:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.97 , 29.40 , 36.70 , 33.99 2024-11-06T04:50:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , 0.02 2024-11-06T04:50:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868629.24 epoch sec; seqNo:2257; 4061711.28 north; 594686.64 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:50:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868628.72 2024-11-06T04:50:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:29Z- waiting for 1730868632.72, time is 1730868629.24 2024-11-06T04:50:29Z- AHRS_M2 msg: 1730868629.52 epoch sec; seqNo:2111 2024-11-06T04:50:29Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.48 psi 2024-11-06T04:50:29Z- handleDepth()- Depth_Keller msg: 1730868629.53 epoch sec; seqNo:2258; depth 50.25 2024-11-06T04:50:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868629.55 epoch sec; seqNo:2258; 4061711.28 north; 594686.64 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T04:50:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868628.72 2024-11-06T04:50:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:29Z- waiting for 1730868632.72, time is 1730868629.55 2024-11-06T04:50:29Z- AHRS_M2 msg: 1730868629.91 epoch sec; seqNo:2112 2024-11-06T04:50:29Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.48 psi 2024-11-06T04:50:29Z- handleDepth()- Depth_Keller msg: 1730868629.92 epoch sec; seqNo:2259; depth 50.26 2024-11-06T04:50:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868629.93 epoch sec; seqNo:2259; 4061711.30 north; 594687.86 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:50:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868628.72 2024-11-06T04:50:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:30Z- waiting for 1730868632.72, time is 1730868629.93 2024-11-06T04:50:30Z- AHRS_M2 msg: 1730868630.32 epoch sec; seqNo:2113 2024-11-06T04:50:30Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.48 psi 2024-11-06T04:50:30Z- handleDepth()- Depth_Keller msg: 1730868630.33 epoch sec; seqNo:2260; depth 50.26 2024-11-06T04:50:30Z- handleDVL() - DVL msg epochMillisec: 1730868630.35 2024-11-06T04:50:30Z- handleDvl() - RDI_Pathfinder msg: 1730868630.35 sec; seqNo:832; #beams:4 2024-11-06T04:50:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.80 , 29.57 , 36.70 , 34.15 2024-11-06T04:50:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.03 2024-11-06T04:50:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868630.36 epoch sec; seqNo:2260; 4061711.30 north; 594687.86 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:50:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868628.72 2024-11-06T04:50:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:30Z- waiting for 1730868632.72, time is 1730868630.36 2024-11-06T04:50:30Z- AHRS_M2 msg: 1730868630.72 epoch sec; seqNo:2114 2024-11-06T04:50:30Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.48 psi 2024-11-06T04:50:30Z- handleDepth()- Depth_Keller msg: 1730868630.73 epoch sec; seqNo:2261; depth 50.28 2024-11-06T04:50:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868630.75 epoch sec; seqNo:2261; 4061711.30 north; 594687.86 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T04:50:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868628.72 2024-11-06T04:50:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:30Z- waiting for 1730868632.72, time is 1730868630.75 2024-11-06T04:50:31Z- AHRS_M2 msg: 1730868631.18 epoch sec; seqNo:2115 2024-11-06T04:50:31Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.48 psi 2024-11-06T04:50:31Z- handleDepth()- Depth_Keller msg: 1730868631.18 epoch sec; seqNo:2262; depth 50.31 2024-11-06T04:50:31Z- handleDVL() - DVL msg epochMillisec: 1730868631.21 2024-11-06T04:50:31Z- handleDvl() - RDI_Pathfinder msg: 1730868631.21 sec; seqNo:833; #beams:4 2024-11-06T04:50:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 29.57 , 36.87 , 34.49 2024-11-06T04:50:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , 0.03 2024-11-06T04:50:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868631.22 epoch sec; seqNo:2262; 4061711.31 north; 594689.08 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:50:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868628.72 2024-11-06T04:50:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:31Z- waiting for 1730868632.72, time is 1730868631.22 2024-11-06T04:50:31Z- AHRS_M2 msg: 1730868631.53 epoch sec; seqNo:2116 2024-11-06T04:50:31Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.48 psi 2024-11-06T04:50:31Z- handleDepth()- Depth_Keller msg: 1730868631.54 epoch sec; seqNo:2263; depth 50.33 2024-11-06T04:50:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868631.56 epoch sec; seqNo:2263; 4061711.69 north; 594689.07 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T04:50:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868628.72 2024-11-06T04:50:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:31Z- waiting for 1730868632.72, time is 1730868631.56 2024-11-06T04:50:31Z- AHRS_M2 msg: 1730868631.93 epoch sec; seqNo:2117 2024-11-06T04:50:31Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.48 psi 2024-11-06T04:50:31Z- handleDepth()- Depth_Keller msg: 1730868631.95 epoch sec; seqNo:2264; depth 50.33 2024-11-06T04:50:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868631.96 epoch sec; seqNo:2264; 4061711.69 north; 594689.07 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T04:50:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868628.72 2024-11-06T04:50:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:32Z- waiting for 1730868632.72, time is 1730868631.96 2024-11-06T04:50:32Z- AHRS_M2 msg: 1730868632.34 epoch sec; seqNo:2118 2024-11-06T04:50:32Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.48 psi 2024-11-06T04:50:32Z- handleDepth()- Depth_Keller msg: 1730868632.35 epoch sec; seqNo:2265; depth 50.34 2024-11-06T04:50:32Z- handleDVL() - DVL msg epochMillisec: 1730868632.37 2024-11-06T04:50:32Z- handleDvl() - RDI_Pathfinder msg: 1730868632.37 sec; seqNo:834; #beams:4 2024-11-06T04:50:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.80 , 29.74 , 37.04 , 34.49 2024-11-06T04:50:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , 0.03 2024-11-06T04:50:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868632.39 epoch sec; seqNo:2265; 4061711.69 north; 594689.07 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:50:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868628.72 2024-11-06T04:50:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:32Z- waiting for 1730868632.72, time is 1730868632.39 2024-11-06T04:50:32Z- AHRS_M2 msg: 1730868632.75 epoch sec; seqNo:2119 2024-11-06T04:50:32Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.49 psi 2024-11-06T04:50:32Z- handleDepth()- Depth_Keller msg: 1730868632.75 epoch sec; seqNo:2266; depth 50.37 2024-11-06T04:50:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868632.77 epoch sec; seqNo:2266; 4061711.70 north; 594690.29 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:50:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868628.72 2024-11-06T04:50:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:32Z- waiting for 1730868632.72, time is 1730868632.77 2024-11-06T04:50:32Z- LcmTrn::updateTrn() >>>> pose time 1730868632.77, meas time 1730868632.37 <<<< 2024-11-06T04:50:32Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868632.77 2024-11-06T04:50:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868632.37 sec, ping # 0; waiting for INS data more recent than 1730868628.72... 2024-11-06T04:50:32Z- TerrainNav::motionUpdate() - currEstimate = (4061712, 594690.29, 50.37) 2024-11-06T04:50:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:50:32Z- TerrainNav:: SubcloudNIS is 0.38, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:50:32Z- TNavPF::Projecting Measurements 2024-11-06T04:50:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:50:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:50:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:50:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:50:32Z- TNavFilter::4 good beams. 2024-11-06T04:50:32Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:50:32Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:50:32Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:50:32Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:50:32Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:50:32Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:50:32Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:50:32Z- TerrainMapOctree: (vehN, vehE ) = (4061711.66, 594689.93). 2024-11-06T04:50:32Z- TerrainMapOctree: Min Distance = 71.26. 2024-11-06T04:50:32Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:50:33Z- Forcing Subcloud Comparison 2024-11-06T04:50:33Z- Weighting particles with subcloud comparison 2024-11-06T04:50:33Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:33Z- meanExpectedMeasDiff: -1.465865 delta_rms_squared: 2.618484 alpha: 0.992420 2024-11-06T04:50:33Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:33Z- meanExpectedMeasDiff: 0.533020 delta_rms_squared: 0.874707 alpha: 0.977646 2024-11-06T04:50:33Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:33Z- meanExpectedMeasDiff: 0.143743 delta_rms_squared: 1.178476 alpha: 0.983312 2024-11-06T04:50:33Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:33Z- meanExpectedMeasDiff: 2.395255 delta_rms_squared: 1.092684 alpha: 0.982025 2024-11-06T04:50:33Z- Subcloud NIS: 0.390258 2024-11-06T04:50:33Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:50:33Z- TNavPF:: sumWeights = 0.217064 2024-11-06T04:50:33Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:50:33Z- TNavPF::First Term of Map Covariance Matrix is 0.68 2024-11-06T04:50:33Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T04:50:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868632.37 sec. 2024-11-06T04:50:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:50:33Z- variances([0], [2], and psi_berg) = 0.469007 0.479572 0.000000 2024-11-06T04:50:33Z- publishEstimatesDecoder() - at 1730868632.77 sec 2024-11-06T04:50:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:50:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061654.61 594697.20 53.23 2024-11-06T04:50:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061655.46 594695.33 53.55 lat/lon: 36.70 -121.94 2024-11-06T04:50:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.47 0.48 0.68 0.00 2024-11-06T04:50:33Z- AHRS_M2 msg: 1730868633.21 epoch sec; seqNo:2120 2024-11-06T04:50:33Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.49 psi 2024-11-06T04:50:33Z- handleDepth()- Depth_Keller msg: 1730868633.22 epoch sec; seqNo:2267; depth 50.38 2024-11-06T04:50:33Z- handleDVL() - DVL msg epochMillisec: 1730868633.26 2024-11-06T04:50:33Z- handleDvl() - RDI_Pathfinder msg: 1730868633.26 sec; seqNo:835; #beams:4 2024-11-06T04:50:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 29.57 , 37.21 , 35.17 2024-11-06T04:50:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , 0.02 2024-11-06T04:50:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868633.27 epoch sec; seqNo:2267; 4061711.70 north; 594690.29 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:50:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868632.77 2024-11-06T04:50:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:33Z- waiting for 1730868636.77, time is 1730868633.27 2024-11-06T04:50:33Z- AHRS_M2 msg: 1730868633.55 epoch sec; seqNo:2121 2024-11-06T04:50:33Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.49 psi 2024-11-06T04:50:33Z- handleDepth()- Depth_Keller msg: 1730868633.56 epoch sec; seqNo:2268; depth 50.38 2024-11-06T04:50:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868633.57 epoch sec; seqNo:2268; 4061711.70 north; 594690.29 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:50:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868632.77 2024-11-06T04:50:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:33Z- waiting for 1730868636.77, time is 1730868633.57 2024-11-06T04:50:33Z- AHRS_M2 msg: 1730868633.95 epoch sec; seqNo:2122 2024-11-06T04:50:33Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.49 psi 2024-11-06T04:50:33Z- handleDepth()- Depth_Keller msg: 1730868633.97 epoch sec; seqNo:2269; depth 50.39 2024-11-06T04:50:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868633.98 epoch sec; seqNo:2269; 4061711.71 north; 594691.51 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:50:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868632.77 2024-11-06T04:50:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:34Z- waiting for 1730868636.77, time is 1730868633.98 2024-11-06T04:50:34Z- AHRS_M2 msg: 1730868634.36 epoch sec; seqNo:2123 2024-11-06T04:50:34Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.49 psi 2024-11-06T04:50:34Z- handleDepth()- Depth_Keller msg: 1730868634.36 epoch sec; seqNo:2270; depth 50.39 2024-11-06T04:50:34Z- handleDVL() - DVL msg epochMillisec: 1730868634.39 2024-11-06T04:50:34Z- handleDvl() - RDI_Pathfinder msg: 1730868634.39 sec; seqNo:836; #beams:4 2024-11-06T04:50:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.80 , 29.57 , 37.04 , 35.34 2024-11-06T04:50:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.03 , 0.02 2024-11-06T04:50:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868634.40 epoch sec; seqNo:2270; 4061711.71 north; 594691.51 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:50:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868632.77 2024-11-06T04:50:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:34Z- waiting for 1730868636.77, time is 1730868634.40 2024-11-06T04:50:34Z- AHRS_M2 msg: 1730868634.76 epoch sec; seqNo:2124 2024-11-06T04:50:34Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.49 psi 2024-11-06T04:50:34Z- handleDepth()- Depth_Keller msg: 1730868634.77 epoch sec; seqNo:2271; depth 50.41 2024-11-06T04:50:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868634.79 epoch sec; seqNo:2271; 4061712.09 north; 594691.51 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T04:50:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868632.77 2024-11-06T04:50:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:34Z- waiting for 1730868636.77, time is 1730868634.79 2024-11-06T04:50:35Z- AHRS_M2 msg: 1730868635.23 epoch sec; seqNo:2125 2024-11-06T04:50:35Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.49 psi 2024-11-06T04:50:35Z- handleDepth()- Depth_Keller msg: 1730868635.24 epoch sec; seqNo:2272; depth 50.42 2024-11-06T04:50:35Z- handleDVL() - DVL msg epochMillisec: 1730868635.26 2024-11-06T04:50:35Z- handleDvl() - RDI_Pathfinder msg: 1730868635.26 sec; seqNo:837; #beams:4 2024-11-06T04:50:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 29.56 , 37.04 , 35.51 2024-11-06T04:50:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , 0.01 2024-11-06T04:50:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868635.27 epoch sec; seqNo:2272; 4061712.11 north; 594692.73 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T04:50:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868632.77 2024-11-06T04:50:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:35Z- waiting for 1730868636.77, time is 1730868635.27 2024-11-06T04:50:35Z- handleDepth()- Depth_Keller msg: 1730868635.59 epoch sec; seqNo:2273; depth 50.42 2024-11-06T04:50:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868635.61 epoch sec; seqNo:2273; 4061712.11 north; 594692.73 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T04:50:35Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:50:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868632.77 2024-11-06T04:50:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:35Z- waiting for 1730868636.77, time is 1730868635.61 2024-11-06T04:50:35Z- AHRS_M2 msg: 1730868635.98 epoch sec; seqNo:2126 2024-11-06T04:50:35Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.49 psi 2024-11-06T04:50:35Z- handleDepth()- Depth_Keller msg: 1730868635.99 epoch sec; seqNo:2274; depth 50.43 2024-11-06T04:50:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868636.00 epoch sec; seqNo:2274; 4061712.11 north; 594692.73 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:50:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868632.77 2024-11-06T04:50:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:36Z- waiting for 1730868636.77, time is 1730868636.00 2024-11-06T04:50:36Z- AHRS_M2 msg: 1730868636.38 epoch sec; seqNo:2127 2024-11-06T04:50:36Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.49 psi 2024-11-06T04:50:36Z- handleDepth()- Depth_Keller msg: 1730868636.39 epoch sec; seqNo:2275; depth 50.43 2024-11-06T04:50:36Z- handleDVL() - DVL msg epochMillisec: 1730868636.42 2024-11-06T04:50:36Z- handleDvl() - RDI_Pathfinder msg: 1730868636.42 sec; seqNo:838; #beams:4 2024-11-06T04:50:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 29.05 , 37.21 , 35.85 2024-11-06T04:50:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , -0.00 2024-11-06T04:50:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868636.43 epoch sec; seqNo:2275; 4061712.11 north; 594692.73 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:50:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868632.77 2024-11-06T04:50:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:36Z- waiting for 1730868636.77, time is 1730868636.43 2024-11-06T04:50:36Z- AHRS_M2 msg: 1730868636.79 epoch sec; seqNo:2128 2024-11-06T04:50:36Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.50 psi 2024-11-06T04:50:36Z- handleDepth()- Depth_Keller msg: 1730868636.81 epoch sec; seqNo:2276; depth 50.43 2024-11-06T04:50:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868636.83 epoch sec; seqNo:2276; 4061712.12 north; 594693.95 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:50:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868632.77 2024-11-06T04:50:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:36Z- waiting for 1730868636.77, time is 1730868636.83 2024-11-06T04:50:36Z- LcmTrn::updateTrn() >>>> pose time 1730868636.83, meas time 1730868636.42 <<<< 2024-11-06T04:50:36Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868636.83 2024-11-06T04:50:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868636.42 sec, ping # 0; waiting for INS data more recent than 1730868632.77... 2024-11-06T04:50:36Z- TerrainNav::motionUpdate() - currEstimate = (4061712, 594693.95, 50.43) 2024-11-06T04:50:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:50:36Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:50:36Z- TNavPF::Projecting Measurements 2024-11-06T04:50:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:50:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:50:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:50:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:50:36Z- TNavFilter::4 good beams. 2024-11-06T04:50:36Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:50:36Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:50:36Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:50:36Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:50:36Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:50:36Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:50:36Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:50:36Z- TerrainMapOctree: (vehN, vehE ) = (4061712.08, 594693.58). 2024-11-06T04:50:36Z- TerrainMapOctree: Min Distance = 67.98. 2024-11-06T04:50:36Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:50:37Z- Forcing Subcloud Comparison 2024-11-06T04:50:37Z- Weighting particles with subcloud comparison 2024-11-06T04:50:37Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:37Z- meanExpectedMeasDiff: -2.120476 delta_rms_squared: 2.246958 alpha: 0.991178 2024-11-06T04:50:37Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:37Z- meanExpectedMeasDiff: -1.903304 delta_rms_squared: 1.004923 alpha: 0.980486 2024-11-06T04:50:37Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:37Z- meanExpectedMeasDiff: 0.107802 delta_rms_squared: 1.248837 alpha: 0.984238 2024-11-06T04:50:37Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:37Z- meanExpectedMeasDiff: 2.621755 delta_rms_squared: 1.294674 alpha: 0.984787 2024-11-06T04:50:37Z- Subcloud NIS: 0.412675 2024-11-06T04:50:37Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:50:37Z- TNavPF:: sumWeights = 0.090779 2024-11-06T04:50:37Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:50:37Z- TNavPF::First Term of Map Covariance Matrix is 0.37 2024-11-06T04:50:37Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T04:50:37Z- TerrainNav::Resampling particle filter... 2024-11-06T04:50:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868636.42 sec. 2024-11-06T04:50:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:50:37Z- variances([0], [2], and psi_berg) = 0.534652 0.551541 0.000000 2024-11-06T04:50:37Z- publishEstimatesDecoder() - at 1730868636.83 sec 2024-11-06T04:50:37Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:50:37Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061656.25 594697.66 53.18 2024-11-06T04:50:37Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061655.79 594698.98 53.76 lat/lon: 36.70 -121.94 2024-11-06T04:50:37Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.53 0.55 0.67 0.00 2024-11-06T04:50:37Z- handleDepth()- Depth_Keller msg: 1730868637.28 epoch sec; seqNo:2277; depth 50.42 2024-11-06T04:50:37Z- handleDVL() - DVL msg epochMillisec: 1730868637.31 2024-11-06T04:50:37Z- handleDvl() - RDI_Pathfinder msg: 1730868637.31 sec; seqNo:839; #beams:4 2024-11-06T04:50:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.80 , 29.39 , 37.38 , 35.85 2024-11-06T04:50:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , -0.02 2024-11-06T04:50:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868637.32 epoch sec; seqNo:2277; 4061712.12 north; 594693.95 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T04:50:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868636.83 2024-11-06T04:50:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:37Z- waiting for 1730868640.83, time is 1730868637.32 2024-11-06T04:50:37Z- AHRS_M2 msg: 1730868637.60 epoch sec; seqNo:2129 2024-11-06T04:50:37Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:50:37Z- handleDepth()- Depth_Keller msg: 1730868637.61 epoch sec; seqNo:2278; depth 50.42 2024-11-06T04:50:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868637.62 epoch sec; seqNo:2278; 4061712.12 north; 594693.95 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T04:50:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868636.83 2024-11-06T04:50:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:37Z- waiting for 1730868640.83, time is 1730868637.62 2024-11-06T04:50:38Z- AHRS_M2 msg: 1730868638.00 epoch sec; seqNo:2130 2024-11-06T04:50:38Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:50:38Z- handleDepth()- Depth_Keller msg: 1730868638.00 epoch sec; seqNo:2279; depth 50.43 2024-11-06T04:50:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868638.02 epoch sec; seqNo:2279; 4061712.13 north; 594695.17 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:50:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868636.83 2024-11-06T04:50:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:38Z- waiting for 1730868640.83, time is 1730868638.02 2024-11-06T04:50:38Z- AHRS_M2 msg: 1730868638.40 epoch sec; seqNo:2131 2024-11-06T04:50:38Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:50:38Z- handleDepth()- Depth_Keller msg: 1730868638.41 epoch sec; seqNo:2280; depth 50.41 2024-11-06T04:50:38Z- handleDVL() - DVL msg epochMillisec: 1730868638.44 2024-11-06T04:50:38Z- handleDvl() - RDI_Pathfinder msg: 1730868638.44 sec; seqNo:840; #beams:4 2024-11-06T04:50:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.29 , 29.39 , 37.38 , 35.68 2024-11-06T04:50:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.01 , -0.04 2024-11-06T04:50:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868638.45 epoch sec; seqNo:2280; 4061712.13 north; 594695.17 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T04:50:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868636.83 2024-11-06T04:50:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:38Z- waiting for 1730868640.83, time is 1730868638.45 2024-11-06T04:50:38Z- AHRS_M2 msg: 1730868638.81 epoch sec; seqNo:2132 2024-11-06T04:50:38Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T04:50:38Z- handleDepth()- Depth_Keller msg: 1730868638.82 epoch sec; seqNo:2281; depth 50.40 2024-11-06T04:50:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868638.84 epoch sec; seqNo:2281; 4061712.13 north; 594695.17 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T04:50:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868636.83 2024-11-06T04:50:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:38Z- waiting for 1730868640.83, time is 1730868638.84 2024-11-06T04:50:39Z- AHRS_M2 msg: 1730868639.23 epoch sec; seqNo:2133 2024-11-06T04:50:39Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 1.51 psi 2024-11-06T04:50:39Z- handleDepth()- Depth_Keller msg: 1730868639.25 epoch sec; seqNo:2282; depth 50.41 2024-11-06T04:50:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868639.29 epoch sec; seqNo:2282; 4061712.15 north; 594696.39 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T04:50:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868636.83 2024-11-06T04:50:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:39Z- waiting for 1730868640.83, time is 1730868639.29 2024-11-06T04:50:39Z- AHRS_M2 msg: 1730868639.61 epoch sec; seqNo:2134 2024-11-06T04:50:39Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 1.51 psi 2024-11-06T04:50:39Z- handleDepth()- Depth_Keller msg: 1730868639.63 epoch sec; seqNo:2283; depth 50.39 2024-11-06T04:50:39Z- handleDVL() - DVL msg epochMillisec: 1730868639.65 2024-11-06T04:50:39Z- handleDvl() - RDI_Pathfinder msg: 1730868639.65 sec; seqNo:841; #beams:4 2024-11-06T04:50:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 29.22 , 37.21 , 35.51 2024-11-06T04:50:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , -0.05 2024-11-06T04:50:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868639.67 epoch sec; seqNo:2283; 4061712.15 north; 594696.39 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:50:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868636.83 2024-11-06T04:50:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:39Z- waiting for 1730868640.83, time is 1730868639.67 2024-11-06T04:50:40Z- AHRS_M2 msg: 1730868640.02 epoch sec; seqNo:2135 2024-11-06T04:50:40Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 1.51 psi 2024-11-06T04:50:40Z- handleDepth()- Depth_Keller msg: 1730868640.03 epoch sec; seqNo:2284; depth 50.39 2024-11-06T04:50:40Z- handleDVL() - DVL msg epochMillisec: 1730868640.03 2024-11-06T04:50:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:50:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:50:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:50:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:50:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:50:40Z- handleDvl() - RDI_Pathfinder msg: 1730868640.03 sec; seqNo:842; #beams:0 2024-11-06T04:50:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:50:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.00 , -0.05 2024-11-06T04:50:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868640.05 epoch sec; seqNo:2284; 4061712.15 north; 594696.39 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:50:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868636.83 2024-11-06T04:50:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:40Z- waiting for 1730868640.83, time is 1730868640.05 2024-11-06T04:50:40Z- AHRS_M2 msg: 1730868640.42 epoch sec; seqNo:2136 2024-11-06T04:50:40Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 1.51 psi 2024-11-06T04:50:40Z- handleDepth()- Depth_Keller msg: 1730868640.43 epoch sec; seqNo:2285; depth 50.39 2024-11-06T04:50:40Z- handleDVL() - DVL msg epochMillisec: 1730868640.46 2024-11-06T04:50:40Z- handleDvl() - RDI_Pathfinder msg: 1730868640.46 sec; seqNo:843; #beams:4 2024-11-06T04:50:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.95 , 29.39 , 37.38 , 35.34 2024-11-06T04:50:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.00 , -0.06 2024-11-06T04:50:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868640.47 epoch sec; seqNo:2285; 4061712.15 north; 594696.39 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:50:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868636.83 2024-11-06T04:50:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:40Z- waiting for 1730868640.83, time is 1730868640.47 2024-11-06T04:50:40Z- AHRS_M2 msg: 1730868640.86 epoch sec; seqNo:2137 2024-11-06T04:50:40Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 1.51 psi 2024-11-06T04:50:40Z- handleDepth()- Depth_Keller msg: 1730868640.86 epoch sec; seqNo:2286; depth 50.38 2024-11-06T04:50:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868640.88 epoch sec; seqNo:2286; 4061712.16 north; 594697.61 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:50:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868636.83 2024-11-06T04:50:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:41Z- waiting for 1730868640.83, time is 1730868640.88 2024-11-06T04:50:41Z- LcmTrn::updateTrn() >>>> pose time 1730868640.88, meas time 1730868640.46 <<<< 2024-11-06T04:50:41Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868640.88 2024-11-06T04:50:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868640.46 sec, ping # 0; waiting for INS data more recent than 1730868636.83... 2024-11-06T04:50:41Z- TerrainNav::motionUpdate() - currEstimate = (4061712, 594697.61, 50.38) 2024-11-06T04:50:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:50:41Z- TerrainNav:: SubcloudNIS is 0.41, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:50:41Z- TNavPF::Projecting Measurements 2024-11-06T04:50:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:50:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:50:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:50:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:50:41Z- TNavFilter::4 good beams. 2024-11-06T04:50:41Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:50:41Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:50:41Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:50:41Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:50:41Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:50:41Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:50:41Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:50:41Z- TerrainMapOctree: (vehN, vehE ) = (4061712.16, 594697.23). 2024-11-06T04:50:41Z- TerrainMapOctree: Min Distance = 64.94. 2024-11-06T04:50:41Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:50:41Z- Forcing Subcloud Comparison 2024-11-06T04:50:41Z- Weighting particles with subcloud comparison 2024-11-06T04:50:41Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:41Z- meanExpectedMeasDiff: -1.745819 delta_rms_squared: 1.721746 alpha: 0.988517 2024-11-06T04:50:41Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:41Z- meanExpectedMeasDiff: -1.607314 delta_rms_squared: 1.039140 alpha: 0.981117 2024-11-06T04:50:41Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:41Z- meanExpectedMeasDiff: -0.048675 delta_rms_squared: 1.257051 alpha: 0.984339 2024-11-06T04:50:41Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:41Z- meanExpectedMeasDiff: 1.480497 delta_rms_squared: 1.228708 alpha: 0.983983 2024-11-06T04:50:41Z- Subcloud NIS: 0.424680 2024-11-06T04:50:41Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:50:41Z- TNavPF:: sumWeights = 0.251494 2024-11-06T04:50:41Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:50:41Z- TNavPF::First Term of Map Covariance Matrix is 0.45 2024-11-06T04:50:41Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T04:50:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868640.46 sec. 2024-11-06T04:50:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:50:41Z- variances([0], [2], and psi_berg) = 0.522870 0.488165 0.000000 2024-11-06T04:50:41Z- publishEstimatesDecoder() - at 1730868640.88 sec 2024-11-06T04:50:41Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:50:41Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061655.99 594702.43 52.95 2024-11-06T04:50:41Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061655.83 594702.62 53.95 lat/lon: 36.70 -121.94 2024-11-06T04:50:41Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.52 0.49 0.64 0.00 2024-11-06T04:50:41Z- AHRS_M2 msg: 1730868641.24 epoch sec; seqNo:2138 2024-11-06T04:50:41Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 1.51 psi 2024-11-06T04:50:41Z- handleDepth()- Depth_Keller msg: 1730868641.26 epoch sec; seqNo:2287; depth 50.38 2024-11-06T04:50:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868641.31 epoch sec; seqNo:2287; 4061712.16 north; 594697.61 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:50:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868640.88 2024-11-06T04:50:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:41Z- waiting for 1730868644.88, time is 1730868641.31 2024-11-06T04:50:41Z- AHRS_M2 msg: 1730868641.64 epoch sec; seqNo:2139 2024-11-06T04:50:41Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 1.51 psi 2024-11-06T04:50:41Z- handleDepth()- Depth_Keller msg: 1730868641.65 epoch sec; seqNo:2288; depth 50.36 2024-11-06T04:50:41Z- handleDVL() - DVL msg epochMillisec: 1730868641.68 2024-11-06T04:50:41Z- handleDvl() - RDI_Pathfinder msg: 1730868641.68 sec; seqNo:844; #beams:4 2024-11-06T04:50:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 29.22 , 37.38 , 35.34 2024-11-06T04:50:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.04 , -0.06 2024-11-06T04:50:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868641.69 epoch sec; seqNo:2288; 4061712.54 north; 594697.61 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:50:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868640.88 2024-11-06T04:50:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:41Z- waiting for 1730868644.88, time is 1730868641.69 2024-11-06T04:50:42Z- AHRS_M2 msg: 1730868642.04 epoch sec; seqNo:2140 2024-11-06T04:50:42Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.51 psi 2024-11-06T04:50:42Z- handleDepth()- Depth_Keller msg: 1730868642.05 epoch sec; seqNo:2289; depth 50.37 2024-11-06T04:50:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868642.06 epoch sec; seqNo:2289; 4061712.54 north; 594697.61 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:50:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868640.88 2024-11-06T04:50:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:42Z- waiting for 1730868644.88, time is 1730868642.06 2024-11-06T04:50:42Z- AHRS_M2 msg: 1730868642.44 epoch sec; seqNo:2141 2024-11-06T04:50:42Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.51 psi 2024-11-06T04:50:42Z- handleDepth()- Depth_Keller msg: 1730868642.45 epoch sec; seqNo:2290; depth 50.36 2024-11-06T04:50:42Z- handleDVL() - DVL msg epochMillisec: 1730868642.47 2024-11-06T04:50:42Z- handleDvl() - RDI_Pathfinder msg: 1730868642.47 sec; seqNo:845; #beams:4 2024-11-06T04:50:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.29 , 29.22 , 37.38 , 35.34 2024-11-06T04:50:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.05 , -0.04 2024-11-06T04:50:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868642.48 epoch sec; seqNo:2290; 4061712.55 north; 594698.83 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:50:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868640.88 2024-11-06T04:50:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:42Z- waiting for 1730868644.88, time is 1730868642.48 2024-11-06T04:50:42Z- AHRS_M2 msg: 1730868642.85 epoch sec; seqNo:2142 2024-11-06T04:50:42Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.51 psi 2024-11-06T04:50:42Z- handleDepth()- Depth_Keller msg: 1730868642.86 epoch sec; seqNo:2291; depth 50.35 2024-11-06T04:50:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868642.88 epoch sec; seqNo:2291; 4061712.55 north; 594698.83 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:50:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868640.88 2024-11-06T04:50:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:43Z- waiting for 1730868644.88, time is 1730868642.88 2024-11-06T04:50:43Z- AHRS_M2 msg: 1730868643.25 epoch sec; seqNo:2143 2024-11-06T04:50:43Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.51 psi 2024-11-06T04:50:43Z- handleDepth()- Depth_Keller msg: 1730868643.26 epoch sec; seqNo:2292; depth 50.37 2024-11-06T04:50:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868643.29 epoch sec; seqNo:2292; 4061712.55 north; 594698.83 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:50:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868640.88 2024-11-06T04:50:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:43Z- waiting for 1730868644.88, time is 1730868643.29 2024-11-06T04:50:43Z- AHRS_M2 msg: 1730868643.65 epoch sec; seqNo:2144 2024-11-06T04:50:43Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.51 psi 2024-11-06T04:50:43Z- handleDepth()- Depth_Keller msg: 1730868643.67 epoch sec; seqNo:2293; depth 50.37 2024-11-06T04:50:43Z- handleDVL() - DVL msg epochMillisec: 1730868643.69 2024-11-06T04:50:43Z- handleDvl() - RDI_Pathfinder msg: 1730868643.69 sec; seqNo:846; #beams:4 2024-11-06T04:50:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 29.22 , 37.55 , 35.34 2024-11-06T04:50:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.07 , -0.00 2024-11-06T04:50:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868643.71 epoch sec; seqNo:2293; 4061712.55 north; 594698.83 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:50:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868640.88 2024-11-06T04:50:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:43Z- waiting for 1730868644.88, time is 1730868643.71 2024-11-06T04:50:44Z- AHRS_M2 msg: 1730868644.06 epoch sec; seqNo:2145 2024-11-06T04:50:44Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.50 psi 2024-11-06T04:50:44Z- handleDepth()- Depth_Keller msg: 1730868644.07 epoch sec; seqNo:2294; depth 50.37 2024-11-06T04:50:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868644.08 epoch sec; seqNo:2294; 4061712.57 north; 594700.05 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:50:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868640.88 2024-11-06T04:50:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:44Z- waiting for 1730868644.88, time is 1730868644.08 2024-11-06T04:50:44Z- AHRS_M2 msg: 1730868644.46 epoch sec; seqNo:2146 2024-11-06T04:50:44Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.50 psi 2024-11-06T04:50:44Z- handleDepth()- Depth_Keller msg: 1730868644.47 epoch sec; seqNo:2295; depth 50.38 2024-11-06T04:50:44Z- handleDVL() - DVL msg epochMillisec: 1730868644.49 2024-11-06T04:50:44Z- handleDvl() - RDI_Pathfinder msg: 1730868644.49 sec; seqNo:847; #beams:4 2024-11-06T04:50:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 29.22 , 37.55 , 35.51 2024-11-06T04:50:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.08 , 0.03 2024-11-06T04:50:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868644.51 epoch sec; seqNo:2295; 4061712.57 north; 594700.05 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:50:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868640.88 2024-11-06T04:50:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:44Z- waiting for 1730868644.88, time is 1730868644.51 2024-11-06T04:50:44Z- AHRS_M2 msg: 1730868644.87 epoch sec; seqNo:2147 2024-11-06T04:50:44Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.50 psi 2024-11-06T04:50:44Z- handleDepth()- Depth_Keller msg: 1730868644.87 epoch sec; seqNo:2296; depth 50.38 2024-11-06T04:50:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868644.88 epoch sec; seqNo:2296; 4061712.57 north; 594700.05 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:50:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868640.88 2024-11-06T04:50:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:45Z- waiting for 1730868644.88, time is 1730868644.88 2024-11-06T04:50:45Z- LcmTrn::updateTrn() >>>> pose time 1730868644.88, meas time 1730868644.49 <<<< 2024-11-06T04:50:45Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868644.88 2024-11-06T04:50:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868644.49 sec, ping # 0; waiting for INS data more recent than 1730868640.88... 2024-11-06T04:50:45Z- TerrainNav::motionUpdate() - currEstimate = (4061713, 594700.05, 50.38) 2024-11-06T04:50:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:50:45Z- TerrainNav:: SubcloudNIS is 0.42, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:50:45Z- TNavPF::Projecting Measurements 2024-11-06T04:50:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:50:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:50:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:50:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:50:45Z- TNavFilter::4 good beams. 2024-11-06T04:50:45Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:50:45Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:50:45Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:50:45Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:50:45Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:50:45Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:50:45Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:50:45Z- TerrainMapOctree: (vehN, vehE ) = (4061712.53, 594699.81). 2024-11-06T04:50:45Z- TerrainMapOctree: Min Distance = 62.63. 2024-11-06T04:50:45Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:50:45Z- Forcing Subcloud Comparison 2024-11-06T04:50:45Z- Weighting particles with subcloud comparison 2024-11-06T04:50:45Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:45Z- meanExpectedMeasDiff: -0.589529 delta_rms_squared: 1.447479 alpha: 0.986371 2024-11-06T04:50:45Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:45Z- meanExpectedMeasDiff: -2.642774 delta_rms_squared: 0.995162 alpha: 0.980299 2024-11-06T04:50:45Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:45Z- meanExpectedMeasDiff: 0.121991 delta_rms_squared: 1.217917 alpha: 0.983844 2024-11-06T04:50:45Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:45Z- meanExpectedMeasDiff: 2.308998 delta_rms_squared: 1.398786 alpha: 0.985903 2024-11-06T04:50:45Z- Subcloud NIS: 0.442680 2024-11-06T04:50:45Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:50:45Z- TNavPF:: sumWeights = 0.108360 2024-11-06T04:50:45Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:50:45Z- TNavPF::First Term of Map Covariance Matrix is 0.09 2024-11-06T04:50:45Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T04:50:45Z- TerrainNav::Resampling particle filter... 2024-11-06T04:50:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868644.49 sec. 2024-11-06T04:50:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:50:45Z- variances([0], [2], and psi_berg) = 0.479819 0.466368 0.000000 2024-11-06T04:50:45Z- publishEstimatesDecoder() - at 1730868644.88 sec 2024-11-06T04:50:45Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:50:45Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061656.74 594703.74 53.85 2024-11-06T04:50:45Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061656.28 594705.01 54.05 lat/lon: 36.70 -121.94 2024-11-06T04:50:45Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.48 0.47 0.65 0.00 2024-11-06T04:50:45Z- AHRS_M2 msg: 1730868645.27 epoch sec; seqNo:2148 2024-11-06T04:50:45Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.50 psi 2024-11-06T04:50:45Z- handleDepth()- Depth_Keller msg: 1730868645.28 epoch sec; seqNo:2297; depth 50.38 2024-11-06T04:50:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868645.30 epoch sec; seqNo:2297; 4061712.95 north; 594700.04 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:50:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868644.88 2024-11-06T04:50:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:45Z- waiting for 1730868648.88, time is 1730868645.30 2024-11-06T04:50:45Z- AHRS_M2 msg: 1730868645.68 epoch sec; seqNo:2149 2024-11-06T04:50:45Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.50 psi 2024-11-06T04:50:45Z- handleDepth()- Depth_Keller msg: 1730868645.68 epoch sec; seqNo:2298; depth 50.38 2024-11-06T04:50:45Z- handleDVL() - DVL msg epochMillisec: 1730868645.70 2024-11-06T04:50:45Z- handleDvl() - RDI_Pathfinder msg: 1730868645.70 sec; seqNo:848; #beams:4 2024-11-06T04:50:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.29 , 29.39 , 37.55 , 35.34 2024-11-06T04:50:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , 0.06 2024-11-06T04:50:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868645.71 epoch sec; seqNo:2298; 4061712.96 north; 594701.26 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:50:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868644.88 2024-11-06T04:50:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:45Z- waiting for 1730868648.88, time is 1730868645.71 2024-11-06T04:50:46Z- AHRS_M2 msg: 1730868646.08 epoch sec; seqNo:2150 2024-11-06T04:50:46Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.50 psi 2024-11-06T04:50:46Z- handleDepth()- Depth_Keller msg: 1730868646.09 epoch sec; seqNo:2299; depth 50.39 2024-11-06T04:50:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868646.11 epoch sec; seqNo:2299; 4061712.96 north; 594701.26 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:50:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868644.88 2024-11-06T04:50:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:46Z- waiting for 1730868648.88, time is 1730868646.11 2024-11-06T04:50:46Z- AHRS_M2 msg: 1730868646.48 epoch sec; seqNo:2151 2024-11-06T04:50:46Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.50 psi 2024-11-06T04:50:46Z- handleDepth()- Depth_Keller msg: 1730868646.49 epoch sec; seqNo:2300; depth 50.39 2024-11-06T04:50:46Z- handleDVL() - DVL msg epochMillisec: 1730868646.51 2024-11-06T04:50:46Z- handleDvl() - RDI_Pathfinder msg: 1730868646.51 sec; seqNo:849; #beams:4 2024-11-06T04:50:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.12 , 29.56 , 37.72 , 35.51 2024-11-06T04:50:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , 0.07 2024-11-06T04:50:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868646.54 epoch sec; seqNo:2300; 4061712.96 north; 594701.26 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:50:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868644.88 2024-11-06T04:50:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:46Z- waiting for 1730868648.88, time is 1730868646.54 2024-11-06T04:50:46Z- AHRS_M2 msg: 1730868646.89 epoch sec; seqNo:2152 2024-11-06T04:50:46Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.50 psi 2024-11-06T04:50:46Z- handleDepth()- Depth_Keller msg: 1730868646.89 epoch sec; seqNo:2301; depth 50.40 2024-11-06T04:50:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868646.91 epoch sec; seqNo:2301; 4061712.97 north; 594702.49 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T04:50:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868644.88 2024-11-06T04:50:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:47Z- waiting for 1730868648.88, time is 1730868646.91 2024-11-06T04:50:47Z- AHRS_M2 msg: 1730868647.29 epoch sec; seqNo:2153 2024-11-06T04:50:47Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.50 psi 2024-11-06T04:50:47Z- handleDepth()- Depth_Keller msg: 1730868647.30 epoch sec; seqNo:2302; depth 50.41 2024-11-06T04:50:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868647.31 epoch sec; seqNo:2302; 4061712.97 north; 594702.49 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T04:50:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868644.88 2024-11-06T04:50:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:47Z- waiting for 1730868648.88, time is 1730868647.31 2024-11-06T04:50:47Z- AHRS_M2 msg: 1730868647.69 epoch sec; seqNo:2154 2024-11-06T04:50:47Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:50:47Z- handleDepth()- Depth_Keller msg: 1730868647.70 epoch sec; seqNo:2303; depth 50.40 2024-11-06T04:50:47Z- handleDVL() - DVL msg epochMillisec: 1730868647.72 2024-11-06T04:50:47Z- handleDvl() - RDI_Pathfinder msg: 1730868647.72 sec; seqNo:850; #beams:4 2024-11-06T04:50:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.12 , 29.56 , 37.89 , 35.34 2024-11-06T04:50:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , 0.05 2024-11-06T04:50:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868647.75 epoch sec; seqNo:2303; 4061712.97 north; 594702.49 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T04:50:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868644.88 2024-11-06T04:50:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:47Z- waiting for 1730868648.88, time is 1730868647.75 2024-11-06T04:50:48Z- AHRS_M2 msg: 1730868648.10 epoch sec; seqNo:2155 2024-11-06T04:50:48Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:50:48Z- handleDepth()- Depth_Keller msg: 1730868648.11 epoch sec; seqNo:2304; depth 50.40 2024-11-06T04:50:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868648.12 epoch sec; seqNo:2304; 4061713.35 north; 594702.48 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T04:50:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868644.88 2024-11-06T04:50:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:48Z- waiting for 1730868648.88, time is 1730868648.12 2024-11-06T04:50:48Z- AHRS_M2 msg: 1730868648.50 epoch sec; seqNo:2156 2024-11-06T04:50:48Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:50:48Z- handleDepth()- Depth_Keller msg: 1730868648.51 epoch sec; seqNo:2305; depth 50.39 2024-11-06T04:50:48Z- handleDVL() - DVL msg epochMillisec: 1730868648.53 2024-11-06T04:50:48Z- handleDvl() - RDI_Pathfinder msg: 1730868648.53 sec; seqNo:851; #beams:4 2024-11-06T04:50:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.29 , 29.73 , 38.06 , 35.34 2024-11-06T04:50:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.02 , 0.02 2024-11-06T04:50:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868648.55 epoch sec; seqNo:2305; 4061713.36 north; 594703.70 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:50:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868644.88 2024-11-06T04:50:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:48Z- waiting for 1730868648.88, time is 1730868648.55 2024-11-06T04:50:48Z- AHRS_M2 msg: 1730868648.91 epoch sec; seqNo:2157 2024-11-06T04:50:48Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.51 psi 2024-11-06T04:50:48Z- handleDepth()- Depth_Keller msg: 1730868648.91 epoch sec; seqNo:2306; depth 50.39 2024-11-06T04:50:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868648.94 epoch sec; seqNo:2306; 4061713.36 north; 594703.70 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:50:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868644.88 2024-11-06T04:50:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:49Z- waiting for 1730868648.88, time is 1730868648.94 2024-11-06T04:50:49Z- LcmTrn::updateTrn() >>>> pose time 1730868648.94, meas time 1730868648.53 <<<< 2024-11-06T04:50:49Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868648.94 2024-11-06T04:50:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868648.53 sec, ping # 0; waiting for INS data more recent than 1730868644.88... 2024-11-06T04:50:49Z- TerrainNav::motionUpdate() - currEstimate = (4061713, 594703.70, 50.39) 2024-11-06T04:50:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:50:49Z- TerrainNav:: SubcloudNIS is 0.44, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:50:49Z- TNavPF::Projecting Measurements 2024-11-06T04:50:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:50:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:50:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:50:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:50:49Z- TNavFilter::4 good beams. 2024-11-06T04:50:49Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:50:49Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:50:49Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:50:49Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:50:49Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:50:49Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:50:49Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:50:49Z- TerrainMapOctree: (vehN, vehE ) = (4061713.29, 594703.34). 2024-11-06T04:50:49Z- TerrainMapOctree: Min Distance = 59.38. 2024-11-06T04:50:49Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:50:49Z- Forcing Subcloud Comparison 2024-11-06T04:50:49Z- Weighting particles with subcloud comparison 2024-11-06T04:50:49Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:49Z- meanExpectedMeasDiff: -0.516727 delta_rms_squared: 1.463798 alpha: 0.986521 2024-11-06T04:50:49Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:49Z- meanExpectedMeasDiff: -2.995482 delta_rms_squared: 1.089366 alpha: 0.981972 2024-11-06T04:50:49Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:49Z- meanExpectedMeasDiff: 0.840545 delta_rms_squared: 1.006983 alpha: 0.980525 2024-11-06T04:50:49Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:49Z- meanExpectedMeasDiff: 2.460927 delta_rms_squared: 1.202588 alpha: 0.983641 2024-11-06T04:50:49Z- Subcloud NIS: 0.469378 2024-11-06T04:50:49Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:50:49Z- TNavPF:: sumWeights = 0.068114 2024-11-06T04:50:49Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:50:49Z- TNavPF::First Term of Map Covariance Matrix is 0.06 2024-11-06T04:50:49Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T04:50:49Z- TerrainNav::Resampling particle filter... 2024-11-06T04:50:49Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868648.53 sec. 2024-11-06T04:50:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:50:49Z- variances([0], [2], and psi_berg) = 0.520944 0.571924 0.000000 2024-11-06T04:50:49Z- publishEstimatesDecoder() - at 1730868648.94 sec 2024-11-06T04:50:49Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:50:49Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061657.47 594707.39 54.64 2024-11-06T04:50:49Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061657.12 594708.54 54.14 lat/lon: 36.70 -121.94 2024-11-06T04:50:49Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.52 0.57 0.70 0.00 2024-11-06T04:50:49Z- AHRS_M2 msg: 1730868649.32 epoch sec; seqNo:2158 2024-11-06T04:50:49Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.51 psi 2024-11-06T04:50:49Z- handleDepth()- Depth_Keller msg: 1730868649.33 epoch sec; seqNo:2307; depth 50.38 2024-11-06T04:50:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868649.34 epoch sec; seqNo:2307; 4061713.36 north; 594703.70 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:50:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868648.94 2024-11-06T04:50:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:49Z- waiting for 1730868652.94, time is 1730868649.34 2024-11-06T04:50:49Z- AHRS_M2 msg: 1730868649.72 epoch sec; seqNo:2159 2024-11-06T04:50:49Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.51 psi 2024-11-06T04:50:49Z- handleDepth()- Depth_Keller msg: 1730868649.73 epoch sec; seqNo:2308; depth 50.38 2024-11-06T04:50:49Z- handleDVL() - DVL msg epochMillisec: 1730868649.74 2024-11-06T04:50:49Z- handleDvl() - RDI_Pathfinder msg: 1730868649.74 sec; seqNo:852; #beams:4 2024-11-06T04:50:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 29.73 , 38.06 , 35.51 2024-11-06T04:50:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , -0.02 2024-11-06T04:50:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868649.76 epoch sec; seqNo:2308; 4061713.38 north; 594704.92 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:50:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868648.94 2024-11-06T04:50:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:49Z- waiting for 1730868652.94, time is 1730868649.76 2024-11-06T04:50:50Z- handleDepth()- Depth_Keller msg: 1730868650.14 epoch sec; seqNo:2309; depth 50.36 2024-11-06T04:50:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868650.15 epoch sec; seqNo:2309; 4061713.38 north; 594704.92 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:50:50Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:50:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868648.94 2024-11-06T04:50:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:50Z- waiting for 1730868652.94, time is 1730868650.15 2024-11-06T04:50:50Z- AHRS_M2 msg: 1730868650.52 epoch sec; seqNo:2160 2024-11-06T04:50:50Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.51 psi 2024-11-06T04:50:50Z- handleDepth()- Depth_Keller msg: 1730868650.53 epoch sec; seqNo:2310; depth 50.35 2024-11-06T04:50:50Z- handleDVL() - DVL msg epochMillisec: 1730868650.55 2024-11-06T04:50:50Z- handleDvl() - RDI_Pathfinder msg: 1730868650.55 sec; seqNo:853; #beams:4 2024-11-06T04:50:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.29 , 29.73 , 38.23 , 35.68 2024-11-06T04:50:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , -0.04 2024-11-06T04:50:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868650.57 epoch sec; seqNo:2310; 4061713.38 north; 594704.92 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:50:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868648.94 2024-11-06T04:50:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:50Z- waiting for 1730868652.94, time is 1730868650.57 2024-11-06T04:50:50Z- AHRS_M2 msg: 1730868650.94 epoch sec; seqNo:2161 2024-11-06T04:50:50Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.51 psi 2024-11-06T04:50:50Z- handleDepth()- Depth_Keller msg: 1730868650.95 epoch sec; seqNo:2311; depth 50.32 2024-11-06T04:50:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868650.96 epoch sec; seqNo:2311; 4061713.39 north; 594706.14 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:50:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868648.94 2024-11-06T04:50:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:51Z- waiting for 1730868652.94, time is 1730868650.96 2024-11-06T04:50:51Z- AHRS_M2 msg: 1730868651.34 epoch sec; seqNo:2162 2024-11-06T04:50:51Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.52 psi 2024-11-06T04:50:51Z- handleDepth()- Depth_Keller msg: 1730868651.34 epoch sec; seqNo:2312; depth 50.33 2024-11-06T04:50:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868651.36 epoch sec; seqNo:2312; 4061713.39 north; 594706.14 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T04:50:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868648.94 2024-11-06T04:50:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:51Z- waiting for 1730868652.94, time is 1730868651.36 2024-11-06T04:50:51Z- handleDepth()- Depth_Keller msg: 1730868651.75 epoch sec; seqNo:2313; depth 50.31 2024-11-06T04:50:51Z- handleDVL() - DVL msg epochMillisec: 1730868651.78 2024-11-06T04:50:51Z- handleDvl() - RDI_Pathfinder msg: 1730868651.78 sec; seqNo:854; #beams:4 2024-11-06T04:50:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.29 , 29.22 , 38.23 , 35.85 2024-11-06T04:50:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , -0.06 2024-11-06T04:50:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868651.80 epoch sec; seqNo:2313; 4061713.39 north; 594706.14 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:50:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868648.94 2024-11-06T04:50:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:51Z- waiting for 1730868652.94, time is 1730868651.80 2024-11-06T04:50:52Z- AHRS_M2 msg: 1730868652.14 epoch sec; seqNo:2163 2024-11-06T04:50:52Z- AHRS_M2 msg: 0.16 phi; 0.03 theta; 1.52 psi 2024-11-06T04:50:52Z- handleDepth()- Depth_Keller msg: 1730868652.14 epoch sec; seqNo:2314; depth 50.29 2024-11-06T04:50:52Z- handleDVL() - DVL msg epochMillisec: 1730868652.15 2024-11-06T04:50:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:50:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:50:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:50:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:50:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:50:52Z- handleDvl() - RDI_Pathfinder msg: 1730868652.15 sec; seqNo:855; #beams:0 2024-11-06T04:50:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:50:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , -0.06 2024-11-06T04:50:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868652.16 epoch sec; seqNo:2314; 4061713.41 north; 594707.36 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:50:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868648.94 2024-11-06T04:50:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:52Z- waiting for 1730868652.94, time is 1730868652.16 2024-11-06T04:50:52Z- AHRS_M2 msg: 1730868652.54 epoch sec; seqNo:2164 2024-11-06T04:50:52Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.52 psi 2024-11-06T04:50:52Z- handleDepth()- Depth_Keller msg: 1730868652.55 epoch sec; seqNo:2315; depth 50.27 2024-11-06T04:50:52Z- handleDVL() - DVL msg epochMillisec: 1730868652.58 2024-11-06T04:50:52Z- handleDvl() - RDI_Pathfinder msg: 1730868652.58 sec; seqNo:856; #beams:4 2024-11-06T04:50:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 29.73 , 38.40 , 35.85 2024-11-06T04:50:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.05 2024-11-06T04:50:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868652.60 epoch sec; seqNo:2315; 4061713.41 north; 594707.36 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T04:50:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868648.94 2024-11-06T04:50:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:52Z- waiting for 1730868652.94, time is 1730868652.60 2024-11-06T04:50:52Z- AHRS_M2 msg: 1730868652.95 epoch sec; seqNo:2165 2024-11-06T04:50:52Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.52 psi 2024-11-06T04:50:52Z- handleDepth()- Depth_Keller msg: 1730868652.96 epoch sec; seqNo:2316; depth 50.25 2024-11-06T04:50:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868652.99 epoch sec; seqNo:2316; 4061713.41 north; 594707.36 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T04:50:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868648.94 2024-11-06T04:50:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:53Z- waiting for 1730868652.94, time is 1730868652.99 2024-11-06T04:50:53Z- LcmTrn::updateTrn() >>>> pose time 1730868652.99, meas time 1730868652.58 <<<< 2024-11-06T04:50:53Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868652.99 2024-11-06T04:50:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868652.58 sec, ping # 0; waiting for INS data more recent than 1730868648.94... 2024-11-06T04:50:53Z- TerrainNav::motionUpdate() - currEstimate = (4061713, 594707.36, 50.25) 2024-11-06T04:50:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:50:53Z- TerrainNav:: SubcloudNIS is 0.47, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:50:53Z- TNavPF::Projecting Measurements 2024-11-06T04:50:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:50:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:50:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:50:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:50:53Z- TNavFilter::4 good beams. 2024-11-06T04:50:53Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:50:53Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:50:53Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:50:53Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:50:53Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:50:53Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:50:53Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:50:53Z- TerrainMapOctree: (vehN, vehE ) = (4061713.40, 594706.99). 2024-11-06T04:50:53Z- TerrainMapOctree: Min Distance = 56.48. 2024-11-06T04:50:53Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:50:53Z- Forcing Subcloud Comparison 2024-11-06T04:50:53Z- Weighting particles with subcloud comparison 2024-11-06T04:50:53Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:53Z- meanExpectedMeasDiff: -0.501456 delta_rms_squared: 1.560104 alpha: 0.987343 2024-11-06T04:50:53Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:53Z- meanExpectedMeasDiff: -3.169448 delta_rms_squared: 1.085525 alpha: 0.981909 2024-11-06T04:50:53Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:53Z- meanExpectedMeasDiff: 2.000649 delta_rms_squared: 1.037042 alpha: 0.981079 2024-11-06T04:50:53Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:50:53Z- meanExpectedMeasDiff: 1.568283 delta_rms_squared: 0.992483 alpha: 0.980247 2024-11-06T04:50:53Z- Subcloud NIS: 0.495819 2024-11-06T04:50:53Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:50:53Z- TNavPF:: sumWeights = 0.063950 2024-11-06T04:50:53Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:50:53Z- TNavPF::First Term of Map Covariance Matrix is 0.05 2024-11-06T04:50:53Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T04:50:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868652.58 sec. 2024-11-06T04:50:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:50:53Z- variances([0], [2], and psi_berg) = 0.513884 0.539551 0.000000 2024-11-06T04:50:53Z- publishEstimatesDecoder() - at 1730868652.99 sec 2024-11-06T04:50:53Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:50:53Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061658.36 594712.29 53.67 2024-11-06T04:50:53Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061657.20 594712.23 54.16 lat/lon: 36.70 -121.94 2024-11-06T04:50:53Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.51 0.54 0.69 0.00 2024-11-06T04:50:53Z- AHRS_M2 msg: 1730868653.35 epoch sec; seqNo:2166 2024-11-06T04:50:53Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.52 psi 2024-11-06T04:50:53Z- handleDepth()- Depth_Keller msg: 1730868653.36 epoch sec; seqNo:2317; depth 50.22 2024-11-06T04:50:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868653.37 epoch sec; seqNo:2317; 4061713.41 north; 594707.36 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T04:50:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868652.99 2024-11-06T04:50:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:53Z- waiting for 1730868656.99, time is 1730868653.37 2024-11-06T04:50:53Z- handleDepth()- Depth_Keller msg: 1730868653.78 epoch sec; seqNo:2318; depth 50.20 2024-11-06T04:50:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868653.80 epoch sec; seqNo:2318; 4061713.42 north; 594708.58 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:50:53Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:50:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868652.99 2024-11-06T04:50:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:53Z- waiting for 1730868656.99, time is 1730868653.80 2024-11-06T04:50:54Z- AHRS_M2 msg: 1730868654.16 epoch sec; seqNo:2167 2024-11-06T04:50:54Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.52 psi 2024-11-06T04:50:54Z- handleDepth()- Depth_Keller msg: 1730868654.18 epoch sec; seqNo:2319; depth 50.20 2024-11-06T04:50:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868654.19 epoch sec; seqNo:2319; 4061713.42 north; 594708.58 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:50:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868652.99 2024-11-06T04:50:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:54Z- waiting for 1730868656.99, time is 1730868654.19 2024-11-06T04:50:54Z- AHRS_M2 msg: 1730868654.57 epoch sec; seqNo:2168 2024-11-06T04:50:54Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.52 psi 2024-11-06T04:50:54Z- handleDepth()- Depth_Keller msg: 1730868654.57 epoch sec; seqNo:2320; depth 50.17 2024-11-06T04:50:54Z- handleDVL() - DVL msg epochMillisec: 1730868654.59 2024-11-06T04:50:54Z- handleDvl() - RDI_Pathfinder msg: 1730868654.59 sec; seqNo:857; #beams:4 2024-11-06T04:50:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 29.90 , 38.23 , 35.85 2024-11-06T04:50:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , -0.02 2024-11-06T04:50:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868654.62 epoch sec; seqNo:2320; 4061713.42 north; 594708.58 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T04:50:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868652.99 2024-11-06T04:50:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:54Z- waiting for 1730868656.99, time is 1730868654.62 2024-11-06T04:50:54Z- handleDepth()- Depth_Keller msg: 1730868654.99 epoch sec; seqNo:2321; depth 50.13 2024-11-06T04:50:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868655.01 epoch sec; seqNo:2321; 4061713.81 north; 594709.80 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T04:50:55Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:50:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868652.99 2024-11-06T04:50:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:55Z- waiting for 1730868656.99, time is 1730868655.01 2024-11-06T04:50:55Z- AHRS_M2 msg: 1730868655.37 epoch sec; seqNo:2169 2024-11-06T04:50:55Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.52 psi 2024-11-06T04:50:55Z- handleDepth()- Depth_Keller msg: 1730868655.40 epoch sec; seqNo:2322; depth 50.12 2024-11-06T04:50:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868655.42 epoch sec; seqNo:2322; 4061713.81 north; 594709.80 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T04:50:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868652.99 2024-11-06T04:50:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:55Z- waiting for 1730868656.99, time is 1730868655.42 2024-11-06T04:50:55Z- AHRS_M2 msg: 1730868655.78 epoch sec; seqNo:2170 2024-11-06T04:50:55Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.52 psi 2024-11-06T04:50:55Z- handleDepth()- Depth_Keller msg: 1730868655.79 epoch sec; seqNo:2323; depth 50.10 2024-11-06T04:50:55Z- handleDVL() - DVL msg epochMillisec: 1730868655.86 2024-11-06T04:50:55Z- handleDvl() - RDI_Pathfinder msg: 1730868655.86 sec; seqNo:858; #beams:4 2024-11-06T04:50:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 30.24 , 38.40 , 35.85 2024-11-06T04:50:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , -0.01 2024-11-06T04:50:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868655.87 epoch sec; seqNo:2323; 4061713.81 north; 594709.80 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T04:50:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868652.99 2024-11-06T04:50:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:55Z- waiting for 1730868656.99, time is 1730868655.87 2024-11-06T04:50:56Z- AHRS_M2 msg: 1730868656.18 epoch sec; seqNo:2171 2024-11-06T04:50:56Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:50:56Z- handleDepth()- Depth_Keller msg: 1730868656.19 epoch sec; seqNo:2324; depth 50.08 2024-11-06T04:50:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868656.23 epoch sec; seqNo:2324; 4061713.81 north; 594709.80 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T04:50:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868652.99 2024-11-06T04:50:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:56Z- waiting for 1730868656.99, time is 1730868656.23 2024-11-06T04:50:56Z- handleDepth()- Depth_Keller msg: 1730868656.61 epoch sec; seqNo:2325; depth 50.07 2024-11-06T04:50:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868656.62 epoch sec; seqNo:2325; 4061713.82 north; 594711.02 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T04:50:56Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:50:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868652.99 2024-11-06T04:50:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:56Z- waiting for 1730868656.99, time is 1730868656.62 2024-11-06T04:50:57Z- AHRS_M2 msg: 1730868656.99 epoch sec; seqNo:2172 2024-11-06T04:50:57Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:50:57Z- handleDepth()- Depth_Keller msg: 1730868656.99 epoch sec; seqNo:2326; depth 50.06 2024-11-06T04:50:57Z- handleDVL() - DVL msg epochMillisec: 1730868656.99 2024-11-06T04:50:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:50:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:50:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:50:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:50:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:50:57Z- handleDvl() - RDI_Pathfinder msg: 1730868656.99 sec; seqNo:859; #beams:0 2024-11-06T04:50:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:50:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , -0.01 2024-11-06T04:50:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868657.02 epoch sec; seqNo:2326; 4061713.82 north; 594711.02 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T04:50:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868652.99 2024-11-06T04:50:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:57Z- waiting for 1730868656.99, time is 1730868657.02 2024-11-06T04:50:57Z- LcmTrn::updateTrn() >>>> pose time 1730868657.02, meas time 1730868656.99 <<<< 2024-11-06T04:50:57Z- LcmTrn::updateTrn() equating pose 1730868656.99 and meas times 1730868656.99 2024-11-06T04:50:57Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868656.99 2024-11-06T04:50:57Z- TerrainNav::motionUpdate() - currEstimate = (4061714, 594711.02, 50.06) 2024-11-06T04:50:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:50:57Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:50:57Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:50:57Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:50:57Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:50:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:50:57Z- variances([0], [2], and psi_berg) = 0.517011 0.543307 0.000000 2024-11-06T04:50:57Z- publishEstimatesDecoder() - at 1730868656.99 sec 2024-11-06T04:50:57Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:50:57Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061658.81 594716.01 53.35 2024-11-06T04:50:57Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061657.62 594715.89 53.98 lat/lon: 36.70 -121.94 2024-11-06T04:50:57Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.52 0.54 0.95 0.00 2024-11-06T04:50:57Z- AHRS_M2 msg: 1730868657.39 epoch sec; seqNo:2173 2024-11-06T04:50:57Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:50:57Z- handleDepth()- Depth_Keller msg: 1730868657.40 epoch sec; seqNo:2327; depth 50.04 2024-11-06T04:50:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868657.41 epoch sec; seqNo:2327; 4061713.82 north; 594711.02 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T04:50:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868656.99 2024-11-06T04:50:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:57Z- waiting for 1730868660.99, time is 1730868657.41 2024-11-06T04:50:57Z- AHRS_M2 msg: 1730868657.80 epoch sec; seqNo:2174 2024-11-06T04:50:57Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:50:57Z- handleDepth()- Depth_Keller msg: 1730868657.80 epoch sec; seqNo:2328; depth 50.03 2024-11-06T04:50:57Z- handleDVL() - DVL msg epochMillisec: 1730868657.83 2024-11-06T04:50:57Z- handleDvl() - RDI_Pathfinder msg: 1730868657.83 sec; seqNo:860; #beams:4 2024-11-06T04:50:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.29 , 30.41 , 38.40 , 36.02 2024-11-06T04:50:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , 0.00 2024-11-06T04:50:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868657.86 epoch sec; seqNo:2328; 4061713.82 north; 594711.02 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T04:50:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868656.99 2024-11-06T04:50:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:57Z- waiting for 1730868660.99, time is 1730868657.86 2024-11-06T04:50:58Z- AHRS_M2 msg: 1730868658.21 epoch sec; seqNo:2175 2024-11-06T04:50:58Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:50:58Z- handleDepth()- Depth_Keller msg: 1730868658.22 epoch sec; seqNo:2329; depth 50.01 2024-11-06T04:50:58Z- handleDVL() - DVL msg epochMillisec: 1730868658.22 2024-11-06T04:50:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:50:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:50:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:50:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:50:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:50:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:50:58Z- handleDvl() - RDI_Pathfinder msg: 1730868658.22 sec; seqNo:861; #beams:0 2024-11-06T04:50:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:50:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , 0.00 2024-11-06T04:50:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868658.24 epoch sec; seqNo:2329; 4061713.84 north; 594712.24 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:50:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868656.99 2024-11-06T04:50:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:58Z- waiting for 1730868660.99, time is 1730868658.24 2024-11-06T04:50:58Z- AHRS_M2 msg: 1730868658.60 epoch sec; seqNo:2176 2024-11-06T04:50:58Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:50:58Z- handleDepth()- Depth_Keller msg: 1730868658.61 epoch sec; seqNo:2330; depth 50.01 2024-11-06T04:50:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868658.62 epoch sec; seqNo:2330; 4061713.84 north; 594712.24 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:50:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868656.99 2024-11-06T04:50:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:58Z- waiting for 1730868660.99, time is 1730868658.62 2024-11-06T04:50:59Z- AHRS_M2 msg: 1730868659.01 epoch sec; seqNo:2177 2024-11-06T04:50:59Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:50:59Z- handleDepth()- Depth_Keller msg: 1730868659.02 epoch sec; seqNo:2331; depth 50.00 2024-11-06T04:50:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868659.03 epoch sec; seqNo:2331; 4061713.84 north; 594712.24 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T04:50:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868656.99 2024-11-06T04:50:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:59Z- waiting for 1730868660.99, time is 1730868659.03 2024-11-06T04:50:59Z- AHRS_M2 msg: 1730868659.41 epoch sec; seqNo:2178 2024-11-06T04:50:59Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:50:59Z- handleDepth()- Depth_Keller msg: 1730868659.42 epoch sec; seqNo:2332; depth 49.99 2024-11-06T04:50:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868659.43 epoch sec; seqNo:2332; 4061713.84 north; 594712.24 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T04:50:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:50:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868656.99 2024-11-06T04:50:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:59Z- waiting for 1730868660.99, time is 1730868659.43 2024-11-06T04:50:59Z- AHRS_M2 msg: 1730868659.82 epoch sec; seqNo:2179 2024-11-06T04:50:59Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:50:59Z- handleDepth()- Depth_Keller msg: 1730868659.83 epoch sec; seqNo:2333; depth 49.97 2024-11-06T04:50:59Z- handleDVL() - DVL msg epochMillisec: 1730868659.86 2024-11-06T04:50:59Z- handleDvl() - RDI_Pathfinder msg: 1730868659.86 sec; seqNo:862; #beams:4 2024-11-06T04:50:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 30.92 , 38.40 , 36.36 2024-11-06T04:50:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , -0.00 2024-11-06T04:50:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868659.87 epoch sec; seqNo:2333; 4061713.85 north; 594713.46 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T04:50:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:50:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868656.99 2024-11-06T04:50:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:50:59Z- waiting for 1730868660.99, time is 1730868659.87 2024-11-06T04:51:00Z- AHRS_M2 msg: 1730868660.22 epoch sec; seqNo:2180 2024-11-06T04:51:00Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:51:00Z- handleDepth()- Depth_Keller msg: 1730868660.23 epoch sec; seqNo:2334; depth 49.96 2024-11-06T04:51:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868660.26 epoch sec; seqNo:2334; 4061714.23 north; 594713.46 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:51:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868656.99 2024-11-06T04:51:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:00Z- waiting for 1730868660.99, time is 1730868660.26 2024-11-06T04:51:00Z- AHRS_M2 msg: 1730868660.62 epoch sec; seqNo:2181 2024-11-06T04:51:00Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:51:00Z- handleDepth()- Depth_Keller msg: 1730868660.63 epoch sec; seqNo:2335; depth 49.95 2024-11-06T04:51:00Z- handleDVL() - DVL msg epochMillisec: 1730868660.64 2024-11-06T04:51:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:51:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:51:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:51:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:51:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:51:00Z- handleDvl() - RDI_Pathfinder msg: 1730868660.64 sec; seqNo:863; #beams:0 2024-11-06T04:51:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:51:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , -0.00 2024-11-06T04:51:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868660.64 epoch sec; seqNo:2335; 4061714.23 north; 594713.46 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T04:51:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868656.99 2024-11-06T04:51:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:00Z- waiting for 1730868660.99, time is 1730868660.64 2024-11-06T04:51:01Z- AHRS_M2 msg: 1730868661.03 epoch sec; seqNo:2182 2024-11-06T04:51:01Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:51:01Z- handleDepth()- Depth_Keller msg: 1730868661.04 epoch sec; seqNo:2336; depth 49.95 2024-11-06T04:51:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868661.05 epoch sec; seqNo:2336; 4061714.24 north; 594714.68 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T04:51:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868656.99 2024-11-06T04:51:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:01Z- waiting for 1730868660.99, time is 1730868661.05 2024-11-06T04:51:01Z- LcmTrn::updateTrn() >>>> pose time 1730868661.05, meas time 1730868660.64 <<<< 2024-11-06T04:51:01Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868661.05 2024-11-06T04:51:01Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:51:01Z- TerrainNav::motionUpdate() - currEstimate = (4061714, 594714.68, 49.95) 2024-11-06T04:51:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:51:01Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:51:01Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:51:01Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:51:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:51:01Z- variances([0], [2], and psi_berg) = 0.519921 0.545575 0.000000 2024-11-06T04:51:01Z- publishEstimatesDecoder() - at 1730868661.05 sec 2024-11-06T04:51:01Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:51:01Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061659.28 594719.63 53.45 2024-11-06T04:51:01Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061658.04 594719.54 53.88 lat/lon: 36.70 -121.94 2024-11-06T04:51:01Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.52 0.55 1.21 0.00 2024-11-06T04:51:01Z- AHRS_M2 msg: 1730868661.43 epoch sec; seqNo:2183 2024-11-06T04:51:01Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:51:01Z- handleDepth()- Depth_Keller msg: 1730868661.44 epoch sec; seqNo:2337; depth 49.94 2024-11-06T04:51:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868661.46 epoch sec; seqNo:2337; 4061714.24 north; 594714.68 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T04:51:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868661.05 2024-11-06T04:51:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:01Z- waiting for 1730868665.05, time is 1730868661.46 2024-11-06T04:51:01Z- AHRS_M2 msg: 1730868661.84 epoch sec; seqNo:2184 2024-11-06T04:51:01Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:51:01Z- handleDepth()- Depth_Keller msg: 1730868661.84 epoch sec; seqNo:2338; depth 49.93 2024-11-06T04:51:01Z- handleDVL() - DVL msg epochMillisec: 1730868661.86 2024-11-06T04:51:01Z- handleDvl() - RDI_Pathfinder msg: 1730868661.86 sec; seqNo:864; #beams:4 2024-11-06T04:51:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 31.26 , 38.40 , 36.53 2024-11-06T04:51:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , 0.01 2024-11-06T04:51:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868661.89 epoch sec; seqNo:2338; 4061714.24 north; 594714.68 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T04:51:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868661.05 2024-11-06T04:51:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:02Z- waiting for 1730868665.05, time is 1730868661.89 2024-11-06T04:51:02Z- AHRS_M2 msg: 1730868662.24 epoch sec; seqNo:2185 2024-11-06T04:51:02Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:51:02Z- handleDepth()- Depth_Keller msg: 1730868662.25 epoch sec; seqNo:2339; depth 49.91 2024-11-06T04:51:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868662.27 epoch sec; seqNo:2339; 4061714.24 north; 594714.68 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:51:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868661.05 2024-11-06T04:51:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:02Z- waiting for 1730868665.05, time is 1730868662.27 2024-11-06T04:51:02Z- AHRS_M2 msg: 1730868662.64 epoch sec; seqNo:2186 2024-11-06T04:51:02Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:51:02Z- handleDepth()- Depth_Keller msg: 1730868662.65 epoch sec; seqNo:2340; depth 49.91 2024-11-06T04:51:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868662.66 epoch sec; seqNo:2340; 4061714.26 north; 594715.90 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:51:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868661.05 2024-11-06T04:51:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:02Z- waiting for 1730868665.05, time is 1730868662.66 2024-11-06T04:51:03Z- AHRS_M2 msg: 1730868663.05 epoch sec; seqNo:2187 2024-11-06T04:51:03Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.53 psi 2024-11-06T04:51:03Z- handleDepth()- Depth_Keller msg: 1730868663.06 epoch sec; seqNo:2341; depth 49.90 2024-11-06T04:51:03Z- handleDVL() - DVL msg epochMillisec: 1730868663.06 2024-11-06T04:51:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:51:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:51:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:51:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:51:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:51:03Z- handleDvl() - RDI_Pathfinder msg: 1730868663.06 sec; seqNo:865; #beams:0 2024-11-06T04:51:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:51:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , 0.01 2024-11-06T04:51:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868663.08 epoch sec; seqNo:2341; 4061714.26 north; 594715.90 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:51:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868661.05 2024-11-06T04:51:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:03Z- waiting for 1730868665.05, time is 1730868663.08 2024-11-06T04:51:03Z- AHRS_M2 msg: 1730868663.45 epoch sec; seqNo:2188 2024-11-06T04:51:03Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.53 psi 2024-11-06T04:51:03Z- handleDepth()- Depth_Keller msg: 1730868663.46 epoch sec; seqNo:2342; depth 49.89 2024-11-06T04:51:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868663.47 epoch sec; seqNo:2342; 4061714.26 north; 594715.90 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:51:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868661.05 2024-11-06T04:51:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:03Z- waiting for 1730868665.05, time is 1730868663.47 2024-11-06T04:51:03Z- handleDepth()- Depth_Keller msg: 1730868663.87 epoch sec; seqNo:2343; depth 49.89 2024-11-06T04:51:03Z- handleDVL() - DVL msg epochMillisec: 1730868663.89 2024-11-06T04:51:03Z- handleDvl() - RDI_Pathfinder msg: 1730868663.89 sec; seqNo:866; #beams:4 2024-11-06T04:51:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 31.43 , 38.23 , 36.87 2024-11-06T04:51:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.03 2024-11-06T04:51:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868663.92 epoch sec; seqNo:2343; 4061714.27 north; 594717.12 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:51:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868661.05 2024-11-06T04:51:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:04Z- waiting for 1730868665.05, time is 1730868663.92 2024-11-06T04:51:04Z- AHRS_M2 msg: 1730868664.26 epoch sec; seqNo:2189 2024-11-06T04:51:04Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.53 psi 2024-11-06T04:51:04Z- handleDepth()- Depth_Keller msg: 1730868664.27 epoch sec; seqNo:2344; depth 49.86 2024-11-06T04:51:04Z- handleDVL() - DVL msg epochMillisec: 1730868664.28 2024-11-06T04:51:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:51:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:51:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:51:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:51:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:51:04Z- handleDvl() - RDI_Pathfinder msg: 1730868664.28 sec; seqNo:867; #beams:0 2024-11-06T04:51:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:51:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.03 2024-11-06T04:51:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868664.29 epoch sec; seqNo:2344; 4061714.27 north; 594717.12 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:51:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868661.05 2024-11-06T04:51:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:04Z- waiting for 1730868665.05, time is 1730868664.29 2024-11-06T04:51:04Z- AHRS_M2 msg: 1730868664.66 epoch sec; seqNo:2190 2024-11-06T04:51:04Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.53 psi 2024-11-06T04:51:04Z- handleDepth()- Depth_Keller msg: 1730868664.67 epoch sec; seqNo:2345; depth 49.85 2024-11-06T04:51:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868664.68 epoch sec; seqNo:2345; 4061714.27 north; 594717.12 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:51:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868661.05 2024-11-06T04:51:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:04Z- waiting for 1730868665.05, time is 1730868664.68 2024-11-06T04:51:05Z- AHRS_M2 msg: 1730868665.07 epoch sec; seqNo:2191 2024-11-06T04:51:05Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.53 psi 2024-11-06T04:51:05Z- handleDepth()- Depth_Keller msg: 1730868665.07 epoch sec; seqNo:2346; depth 49.85 2024-11-06T04:51:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868665.08 epoch sec; seqNo:2346; 4061714.65 north; 594717.11 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:51:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868661.05 2024-11-06T04:51:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:05Z- waiting for 1730868665.05, time is 1730868665.08 2024-11-06T04:51:05Z- LcmTrn::updateTrn() >>>> pose time 1730868665.08, meas time 1730868664.28 <<<< 2024-11-06T04:51:05Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868665.08 2024-11-06T04:51:05Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:51:05Z- TerrainNav::motionUpdate() - currEstimate = (4061715, 594717.11, 49.85) 2024-11-06T04:51:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:51:05Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:51:05Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:51:05Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:51:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:51:05Z- variances([0], [2], and psi_berg) = 0.520926 0.547325 0.000000 2024-11-06T04:51:05Z- publishEstimatesDecoder() - at 1730868665.08 sec 2024-11-06T04:51:05Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:51:05Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061659.67 594722.02 53.39 2024-11-06T04:51:05Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061658.44 594721.98 53.77 lat/lon: 36.70 -121.94 2024-11-06T04:51:05Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.52 0.55 1.47 0.00 2024-11-06T04:51:05Z- AHRS_M2 msg: 1730868665.47 epoch sec; seqNo:2192 2024-11-06T04:51:05Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.53 psi 2024-11-06T04:51:05Z- handleDepth()- Depth_Keller msg: 1730868665.49 epoch sec; seqNo:2347; depth 49.82 2024-11-06T04:51:05Z- handleDVL() - DVL msg epochMillisec: 1730868665.49 2024-11-06T04:51:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:51:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:51:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:51:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:51:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:51:05Z- handleDvl() - RDI_Pathfinder msg: 1730868665.49 sec; seqNo:868; #beams:0 2024-11-06T04:51:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:51:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.03 2024-11-06T04:51:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868665.50 epoch sec; seqNo:2347; 4061714.66 north; 594718.33 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:51:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868665.08 2024-11-06T04:51:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:05Z- waiting for 1730868669.08, time is 1730868665.50 2024-11-06T04:51:05Z- AHRS_M2 msg: 1730868665.88 epoch sec; seqNo:2193 2024-11-06T04:51:05Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.53 psi 2024-11-06T04:51:05Z- handleDepth()- Depth_Keller msg: 1730868665.89 epoch sec; seqNo:2348; depth 49.80 2024-11-06T04:51:05Z- handleDVL() - DVL msg epochMillisec: 1730868665.92 2024-11-06T04:51:05Z- handleDvl() - RDI_Pathfinder msg: 1730868665.92 sec; seqNo:869; #beams:4 2024-11-06T04:51:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.29 , 31.94 , 38.23 , 36.87 2024-11-06T04:51:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , -0.00 2024-11-06T04:51:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868665.93 epoch sec; seqNo:2348; 4061714.66 north; 594718.33 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T04:51:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868665.08 2024-11-06T04:51:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:06Z- waiting for 1730868669.08, time is 1730868665.93 2024-11-06T04:51:06Z- AHRS_M2 msg: 1730868666.29 epoch sec; seqNo:2194 2024-11-06T04:51:06Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.53 psi 2024-11-06T04:51:06Z- handleDepth()- Depth_Keller msg: 1730868666.29 epoch sec; seqNo:2349; depth 49.80 2024-11-06T04:51:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868666.31 epoch sec; seqNo:2349; 4061714.66 north; 594718.33 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T04:51:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868665.08 2024-11-06T04:51:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:06Z- waiting for 1730868669.08, time is 1730868666.31 2024-11-06T04:51:06Z- AHRS_M2 msg: 1730868666.68 epoch sec; seqNo:2195 2024-11-06T04:51:06Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.53 psi 2024-11-06T04:51:06Z- handleDepth()- Depth_Keller msg: 1730868666.69 epoch sec; seqNo:2350; depth 49.77 2024-11-06T04:51:06Z- handleDVL() - DVL msg epochMillisec: 1730868666.69 2024-11-06T04:51:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:51:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:51:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:51:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:51:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:51:06Z- handleDvl() - RDI_Pathfinder msg: 1730868666.69 sec; seqNo:870; #beams:0 2024-11-06T04:51:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:51:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , -0.00 2024-11-06T04:51:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868666.71 epoch sec; seqNo:2350; 4061714.68 north; 594719.55 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T04:51:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868665.08 2024-11-06T04:51:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:06Z- waiting for 1730868669.08, time is 1730868666.71 2024-11-06T04:51:07Z- AHRS_M2 msg: 1730868667.09 epoch sec; seqNo:2196 2024-11-06T04:51:07Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.53 psi 2024-11-06T04:51:07Z- handleDepth()- Depth_Keller msg: 1730868667.09 epoch sec; seqNo:2351; depth 49.75 2024-11-06T04:51:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868667.11 epoch sec; seqNo:2351; 4061714.68 north; 594719.55 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:51:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868665.08 2024-11-06T04:51:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:07Z- waiting for 1730868669.08, time is 1730868667.11 2024-11-06T04:51:07Z- AHRS_M2 msg: 1730868667.49 epoch sec; seqNo:2197 2024-11-06T04:51:07Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.54 psi 2024-11-06T04:51:07Z- handleDepth()- Depth_Keller msg: 1730868667.50 epoch sec; seqNo:2352; depth 49.75 2024-11-06T04:51:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868667.52 epoch sec; seqNo:2352; 4061714.68 north; 594719.55 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:51:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868665.08 2024-11-06T04:51:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:07Z- waiting for 1730868669.08, time is 1730868667.52 2024-11-06T04:51:07Z- AHRS_M2 msg: 1730868667.89 epoch sec; seqNo:2198 2024-11-06T04:51:07Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.54 psi 2024-11-06T04:51:07Z- handleDepth()- Depth_Keller msg: 1730868667.91 epoch sec; seqNo:2353; depth 49.72 2024-11-06T04:51:07Z- handleDVL() - DVL msg epochMillisec: 1730868667.93 2024-11-06T04:51:07Z- handleDvl() - RDI_Pathfinder msg: 1730868667.93 sec; seqNo:871; #beams:4 2024-11-06T04:51:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 32.28 , 38.23 , 37.04 2024-11-06T04:51:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , -0.05 2024-11-06T04:51:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868667.95 epoch sec; seqNo:2353; 4061714.69 north; 594720.77 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:51:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868665.08 2024-11-06T04:51:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:08Z- waiting for 1730868669.08, time is 1730868667.95 2024-11-06T04:51:08Z- AHRS_M2 msg: 1730868668.30 epoch sec; seqNo:2199 2024-11-06T04:51:08Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.54 psi 2024-11-06T04:51:08Z- handleDepth()- Depth_Keller msg: 1730868668.31 epoch sec; seqNo:2354; depth 49.73 2024-11-06T04:51:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868668.33 epoch sec; seqNo:2354; 4061714.69 north; 594720.77 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T04:51:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868665.08 2024-11-06T04:51:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:08Z- waiting for 1730868669.08, time is 1730868668.33 2024-11-06T04:51:08Z- AHRS_M2 msg: 1730868668.70 epoch sec; seqNo:2200 2024-11-06T04:51:08Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.54 psi 2024-11-06T04:51:08Z- handleDepth()- Depth_Keller msg: 1730868668.71 epoch sec; seqNo:2355; depth 49.71 2024-11-06T04:51:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868668.72 epoch sec; seqNo:2355; 4061714.69 north; 594720.77 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T04:51:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868665.08 2024-11-06T04:51:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:08Z- waiting for 1730868669.08, time is 1730868668.72 2024-11-06T04:51:09Z- AHRS_M2 msg: 1730868669.11 epoch sec; seqNo:2201 2024-11-06T04:51:09Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.54 psi 2024-11-06T04:51:09Z- handleDepth()- Depth_Keller msg: 1730868669.11 epoch sec; seqNo:2356; depth 49.69 2024-11-06T04:51:09Z- handleDVL() - DVL msg epochMillisec: 1730868669.12 2024-11-06T04:51:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:51:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:51:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:51:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:51:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:51:09Z- handleDvl() - RDI_Pathfinder msg: 1730868669.12 sec; seqNo:872; #beams:0 2024-11-06T04:51:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:51:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , -0.05 2024-11-06T04:51:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868669.12 epoch sec; seqNo:2356; 4061714.69 north; 594720.77 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T04:51:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868665.08 2024-11-06T04:51:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:09Z- waiting for 1730868669.08, time is 1730868669.12 2024-11-06T04:51:09Z- LcmTrn::updateTrn() >>>> pose time 1730868669.12, meas time 1730868669.12 <<<< 2024-11-06T04:51:09Z- LcmTrn::updateTrn() equating pose 1730868669.12 and meas times 1730868669.12 2024-11-06T04:51:09Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868669.12 2024-11-06T04:51:09Z- TerrainNav::motionUpdate() - currEstimate = (4061715, 594720.77, 49.69) 2024-11-06T04:51:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:51:09Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:51:09Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:51:09Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:51:09Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:51:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:51:09Z- variances([0], [2], and psi_berg) = 0.523825 0.550982 0.000000 2024-11-06T04:51:09Z- publishEstimatesDecoder() - at 1730868669.12 sec 2024-11-06T04:51:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:51:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061659.73 594725.65 53.72 2024-11-06T04:51:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061658.48 594725.64 53.62 lat/lon: 36.70 -121.94 2024-11-06T04:51:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.52 0.55 1.70 0.00 2024-11-06T04:51:09Z- AHRS_M2 msg: 1730868669.51 epoch sec; seqNo:2202 2024-11-06T04:51:09Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.54 psi 2024-11-06T04:51:09Z- handleDepth()- Depth_Keller msg: 1730868669.52 epoch sec; seqNo:2357; depth 49.69 2024-11-06T04:51:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868669.54 epoch sec; seqNo:2357; 4061714.70 north; 594721.99 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T04:51:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868669.12 2024-11-06T04:51:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:09Z- waiting for 1730868673.12, time is 1730868669.54 2024-11-06T04:51:09Z- AHRS_M2 msg: 1730868669.91 epoch sec; seqNo:2203 2024-11-06T04:51:09Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.54 psi 2024-11-06T04:51:09Z- handleDepth()- Depth_Keller msg: 1730868669.92 epoch sec; seqNo:2358; depth 49.67 2024-11-06T04:51:09Z- handleDVL() - DVL msg epochMillisec: 1730868669.95 2024-11-06T04:51:09Z- handleDvl() - RDI_Pathfinder msg: 1730868669.95 sec; seqNo:873; #beams:4 2024-11-06T04:51:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.80 , 32.79 , 37.89 , 37.38 2024-11-06T04:51:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , -0.06 2024-11-06T04:51:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868669.97 epoch sec; seqNo:2358; 4061714.70 north; 594721.99 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:51:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868669.12 2024-11-06T04:51:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:10Z- waiting for 1730868673.12, time is 1730868669.97 2024-11-06T04:51:10Z- AHRS_M2 msg: 1730868670.33 epoch sec; seqNo:2204 2024-11-06T04:51:10Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.53 psi 2024-11-06T04:51:10Z- handleDepth()- Depth_Keller msg: 1730868670.33 epoch sec; seqNo:2359; depth 49.66 2024-11-06T04:51:10Z- handleDVL() - DVL msg epochMillisec: 1730868670.33 2024-11-06T04:51:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:51:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:51:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:51:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:51:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:51:10Z- handleDvl() - RDI_Pathfinder msg: 1730868670.33 sec; seqNo:874; #beams:0 2024-11-06T04:51:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:51:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , -0.06 2024-11-06T04:51:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868670.35 epoch sec; seqNo:2359; 4061714.70 north; 594721.99 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:51:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868669.12 2024-11-06T04:51:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:10Z- waiting for 1730868673.12, time is 1730868670.35 2024-11-06T04:51:10Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:51:10Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:51:10Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:51:10Z- AHRS_M2 msg: 1730868670.72 epoch sec; seqNo:2205 2024-11-06T04:51:10Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.53 psi 2024-11-06T04:51:10Z- handleDepth()- Depth_Keller msg: 1730868670.75 epoch sec; seqNo:2360; depth 49.64 2024-11-06T04:51:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868670.76 epoch sec; seqNo:2360; 4061714.70 north; 594721.99 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:51:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868669.12 2024-11-06T04:51:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:10Z- waiting for 1730868673.12, time is 1730868670.76 2024-11-06T04:51:11Z- AHRS_M2 msg: 1730868671.13 epoch sec; seqNo:2206 2024-11-06T04:51:11Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.53 psi 2024-11-06T04:51:11Z- handleDepth()- Depth_Keller msg: 1730868671.14 epoch sec; seqNo:2361; depth 49.65 2024-11-06T04:51:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868671.16 epoch sec; seqNo:2361; 4061714.72 north; 594723.21 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:51:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868669.12 2024-11-06T04:51:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:11Z- waiting for 1730868673.12, time is 1730868671.16 2024-11-06T04:51:11Z- AHRS_M2 msg: 1730868671.54 epoch sec; seqNo:2207 2024-11-06T04:51:11Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.52 psi 2024-11-06T04:51:11Z- handleDepth()- Depth_Keller msg: 1730868671.54 epoch sec; seqNo:2362; depth 49.64 2024-11-06T04:51:11Z- handleDVL() - DVL msg epochMillisec: 1730868671.54 2024-11-06T04:51:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:51:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:51:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:51:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:51:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:51:11Z- handleDvl() - RDI_Pathfinder msg: 1730868671.54 sec; seqNo:875; #beams:0 2024-11-06T04:51:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:51:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.04 , -0.06 2024-11-06T04:51:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868671.55 epoch sec; seqNo:2362; 4061714.72 north; 594723.21 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:51:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868669.12 2024-11-06T04:51:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:11Z- waiting for 1730868673.12, time is 1730868671.55 2024-11-06T04:51:11Z- AHRS_M2 msg: 1730868671.94 epoch sec; seqNo:2208 2024-11-06T04:51:11Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.51 psi 2024-11-06T04:51:11Z- handleDepth()- Depth_Keller msg: 1730868671.95 epoch sec; seqNo:2363; depth 49.63 2024-11-06T04:51:11Z- handleDVL() - DVL msg epochMillisec: 1730868671.97 2024-11-06T04:51:11Z- handleDvl() - RDI_Pathfinder msg: 1730868671.97 sec; seqNo:876; #beams:4 2024-11-06T04:51:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 32.62 , 37.55 , 37.55 2024-11-06T04:51:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.08 , -0.00 2024-11-06T04:51:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868671.98 epoch sec; seqNo:2363; 4061714.72 north; 594723.21 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:51:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868669.12 2024-11-06T04:51:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:12Z- waiting for 1730868673.12, time is 1730868671.98 2024-11-06T04:51:12Z- AHRS_M2 msg: 1730868672.34 epoch sec; seqNo:2209 2024-11-06T04:51:12Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.51 psi 2024-11-06T04:51:12Z- handleDepth()- Depth_Keller msg: 1730868672.35 epoch sec; seqNo:2364; depth 49.62 2024-11-06T04:51:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868672.38 epoch sec; seqNo:2364; 4061714.73 north; 594724.43 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:51:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868669.12 2024-11-06T04:51:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:12Z- waiting for 1730868673.12, time is 1730868672.38 2024-11-06T04:51:12Z- AHRS_M2 msg: 1730868672.74 epoch sec; seqNo:2210 2024-11-06T04:51:12Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.51 psi 2024-11-06T04:51:12Z- handleDepth()- Depth_Keller msg: 1730868672.76 epoch sec; seqNo:2365; depth 49.63 2024-11-06T04:51:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868672.76 epoch sec; seqNo:2365; 4061715.11 north; 594724.43 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:51:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868669.12 2024-11-06T04:51:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:12Z- waiting for 1730868673.12, time is 1730868672.76 2024-11-06T04:51:13Z- AHRS_M2 msg: 1730868673.15 epoch sec; seqNo:2211 2024-11-06T04:51:13Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.50 psi 2024-11-06T04:51:13Z- handleDepth()- Depth_Keller msg: 1730868673.16 epoch sec; seqNo:2366; depth 49.63 2024-11-06T04:51:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868673.17 epoch sec; seqNo:2366; 4061715.11 north; 594724.43 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:51:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868669.12 2024-11-06T04:51:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:13Z- waiting for 1730868673.12, time is 1730868673.17 2024-11-06T04:51:13Z- LcmTrn::updateTrn() >>>> pose time 1730868673.17, meas time 1730868671.97 <<<< 2024-11-06T04:51:13Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868673.17 2024-11-06T04:51:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868671.97 sec, ping # 0; waiting for INS data more recent than 1730868669.12... 2024-11-06T04:51:13Z- TerrainNav::motionUpdate() - currEstimate = (4061715, 594724.43, 49.63) 2024-11-06T04:51:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:51:13Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:51:13Z- TNavPF::Projecting Measurements 2024-11-06T04:51:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:51:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:51:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:51:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:51:13Z- TNavFilter::4 good beams. 2024-11-06T04:51:13Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:51:13Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:51:13Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:51:13Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:51:13Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:51:13Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:51:13Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:51:13Z- TerrainMapOctree: (vehN, vehE ) = (4061714.99, 594723.34). 2024-11-06T04:51:13Z- TerrainMapOctree: Min Distance = 44.01. 2024-11-06T04:51:13Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:51:13Z- Forcing Subcloud Comparison 2024-11-06T04:51:13Z- Weighting particles with subcloud comparison 2024-11-06T04:51:13Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:13Z- meanExpectedMeasDiff: 3.722038 delta_rms_squared: 3.541188 alpha: 0.994384 2024-11-06T04:51:13Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:13Z- meanExpectedMeasDiff: -1.868882 delta_rms_squared: 1.972461 alpha: 0.989962 2024-11-06T04:51:13Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:13Z- meanExpectedMeasDiff: 2.241696 delta_rms_squared: 2.701329 alpha: 0.992651 2024-11-06T04:51:13Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:13Z- meanExpectedMeasDiff: 0.365657 delta_rms_squared: 2.634092 alpha: 0.992464 2024-11-06T04:51:13Z- Subcloud NIS: 0.534632 2024-11-06T04:51:13Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:51:13Z- TNavPF:: sumWeights = 0.061007 2024-11-06T04:51:13Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:51:13Z- TNavPF::First Term of Map Covariance Matrix is 0.44 2024-11-06T04:51:13Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T04:51:13Z- TerrainNav::Resampling particle filter... 2024-11-06T04:51:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868671.97 sec. 2024-11-06T04:51:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:51:13Z- variances([0], [2], and psi_berg) = 0.543311 0.383198 0.000000 2024-11-06T04:51:13Z- publishEstimatesDecoder() - at 1730868673.17 sec 2024-11-06T04:51:13Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:51:13Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061659.20 594728.19 51.36 2024-11-06T04:51:13Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061658.78 594729.23 52.80 lat/lon: 36.70 -121.94 2024-11-06T04:51:13Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.54 0.38 0.83 0.00 2024-11-06T04:51:13Z- AHRS_M2 msg: 1730868673.56 epoch sec; seqNo:2212 2024-11-06T04:51:13Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.50 psi 2024-11-06T04:51:13Z- handleDepth()- Depth_Keller msg: 1730868673.56 epoch sec; seqNo:2367; depth 49.62 2024-11-06T04:51:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868673.59 epoch sec; seqNo:2367; 4061715.11 north; 594724.43 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:51:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868673.17 2024-11-06T04:51:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:13Z- waiting for 1730868677.17, time is 1730868673.59 2024-11-06T04:51:13Z- handleDepth()- Depth_Keller msg: 1730868673.97 epoch sec; seqNo:2368; depth 49.61 2024-11-06T04:51:14Z- handleDVL() - DVL msg epochMillisec: 1730868673.99 2024-11-06T04:51:14Z- handleDvl() - RDI_Pathfinder msg: 1730868673.99 sec; seqNo:877; #beams:4 2024-11-06T04:51:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 32.62 , 37.72 , 37.38 2024-11-06T04:51:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.05 , 0.05 2024-11-06T04:51:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868674.01 epoch sec; seqNo:2368; 4061715.12 north; 594725.65 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:51:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868673.17 2024-11-06T04:51:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:14Z- waiting for 1730868677.17, time is 1730868674.01 2024-11-06T04:51:14Z- AHRS_M2 msg: 1730868674.36 epoch sec; seqNo:2213 2024-11-06T04:51:14Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 1.49 psi 2024-11-06T04:51:14Z- handleDepth()- Depth_Keller msg: 1730868674.37 epoch sec; seqNo:2369; depth 49.62 2024-11-06T04:51:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868674.38 epoch sec; seqNo:2369; 4061715.12 north; 594725.65 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:51:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868673.17 2024-11-06T04:51:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:14Z- waiting for 1730868677.17, time is 1730868674.38 2024-11-06T04:51:14Z- AHRS_M2 msg: 1730868674.76 epoch sec; seqNo:2214 2024-11-06T04:51:14Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.49 psi 2024-11-06T04:51:14Z- handleDepth()- Depth_Keller msg: 1730868674.77 epoch sec; seqNo:2370; depth 49.62 2024-11-06T04:51:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868674.78 epoch sec; seqNo:2370; 4061715.12 north; 594725.65 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:51:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868673.17 2024-11-06T04:51:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:14Z- waiting for 1730868677.17, time is 1730868674.78 2024-11-06T04:51:15Z- AHRS_M2 msg: 1730868675.17 epoch sec; seqNo:2215 2024-11-06T04:51:15Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.49 psi 2024-11-06T04:51:15Z- handleDepth()- Depth_Keller msg: 1730868675.17 epoch sec; seqNo:2371; depth 49.63 2024-11-06T04:51:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868675.18 epoch sec; seqNo:2371; 4061715.12 north; 594725.65 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:51:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868673.17 2024-11-06T04:51:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:15Z- waiting for 1730868677.17, time is 1730868675.18 2024-11-06T04:51:15Z- AHRS_M2 msg: 1730868675.57 epoch sec; seqNo:2216 2024-11-06T04:51:15Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.49 psi 2024-11-06T04:51:15Z- handleDepth()- Depth_Keller msg: 1730868675.58 epoch sec; seqNo:2372; depth 49.61 2024-11-06T04:51:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868675.59 epoch sec; seqNo:2372; 4061715.52 north; 594726.87 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:51:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868673.17 2024-11-06T04:51:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:15Z- waiting for 1730868677.17, time is 1730868675.59 2024-11-06T04:51:15Z- AHRS_M2 msg: 1730868675.98 epoch sec; seqNo:2217 2024-11-06T04:51:15Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.49 psi 2024-11-06T04:51:15Z- handleDepth()- Depth_Keller msg: 1730868675.99 epoch sec; seqNo:2373; depth 49.63 2024-11-06T04:51:16Z- handleDVL() - DVL msg epochMillisec: 1730868676.02 2024-11-06T04:51:16Z- handleDvl() - RDI_Pathfinder msg: 1730868676.02 sec; seqNo:878; #beams:4 2024-11-06T04:51:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.29 , 33.13 , 37.38 , 37.55 2024-11-06T04:51:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.03 2024-11-06T04:51:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868676.03 epoch sec; seqNo:2373; 4061715.52 north; 594726.87 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:51:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868673.17 2024-11-06T04:51:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:16Z- waiting for 1730868677.17, time is 1730868676.03 2024-11-06T04:51:16Z- AHRS_M2 msg: 1730868676.38 epoch sec; seqNo:2218 2024-11-06T04:51:16Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.49 psi 2024-11-06T04:51:16Z- handleDepth()- Depth_Keller msg: 1730868676.39 epoch sec; seqNo:2374; depth 49.60 2024-11-06T04:51:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868676.40 epoch sec; seqNo:2374; 4061715.52 north; 594726.87 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:51:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868673.17 2024-11-06T04:51:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:16Z- waiting for 1730868677.17, time is 1730868676.40 2024-11-06T04:51:16Z- AHRS_M2 msg: 1730868676.78 epoch sec; seqNo:2219 2024-11-06T04:51:16Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.49 psi 2024-11-06T04:51:16Z- handleDepth()- Depth_Keller msg: 1730868676.79 epoch sec; seqNo:2375; depth 49.60 2024-11-06T04:51:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868676.80 epoch sec; seqNo:2375; 4061715.53 north; 594728.09 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:51:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868673.17 2024-11-06T04:51:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:16Z- waiting for 1730868677.17, time is 1730868676.80 2024-11-06T04:51:17Z- AHRS_M2 msg: 1730868677.19 epoch sec; seqNo:2220 2024-11-06T04:51:17Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.49 psi 2024-11-06T04:51:17Z- handleDepth()- Depth_Keller msg: 1730868677.19 epoch sec; seqNo:2376; depth 49.61 2024-11-06T04:51:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868677.20 epoch sec; seqNo:2376; 4061715.53 north; 594728.09 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:51:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868673.17 2024-11-06T04:51:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:17Z- waiting for 1730868677.17, time is 1730868677.20 2024-11-06T04:51:17Z- LcmTrn::updateTrn() >>>> pose time 1730868677.20, meas time 1730868676.02 <<<< 2024-11-06T04:51:17Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868677.20 2024-11-06T04:51:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868676.02 sec, ping # 0; waiting for INS data more recent than 1730868673.17... 2024-11-06T04:51:17Z- TerrainNav::motionUpdate() - currEstimate = (4061716, 594728.09, 49.61) 2024-11-06T04:51:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:51:17Z- TerrainNav:: SubcloudNIS is 0.53, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:51:17Z- TNavPF::Projecting Measurements 2024-11-06T04:51:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:51:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:51:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:51:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:51:17Z- TNavFilter::4 good beams. 2024-11-06T04:51:17Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:51:17Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:51:17Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:51:17Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:51:17Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:51:17Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:51:17Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:51:17Z- TerrainMapOctree: (vehN, vehE ) = (4061715.41, 594727.01). 2024-11-06T04:51:17Z- TerrainMapOctree: Min Distance = 41.53. 2024-11-06T04:51:17Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:51:17Z- Forcing Subcloud Comparison 2024-11-06T04:51:17Z- Weighting particles with subcloud comparison 2024-11-06T04:51:17Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:17Z- meanExpectedMeasDiff: 2.940493 delta_rms_squared: 2.996810 alpha: 0.993370 2024-11-06T04:51:17Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:17Z- meanExpectedMeasDiff: -2.383793 delta_rms_squared: 1.257065 alpha: 0.984339 2024-11-06T04:51:17Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:17Z- meanExpectedMeasDiff: 1.350876 delta_rms_squared: 1.468122 alpha: 0.986560 2024-11-06T04:51:17Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:17Z- meanExpectedMeasDiff: -0.800278 delta_rms_squared: 1.467305 alpha: 0.986553 2024-11-06T04:51:17Z- Subcloud NIS: 0.541352 2024-11-06T04:51:17Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:51:17Z- TNavPF:: sumWeights = 0.079743 2024-11-06T04:51:17Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:51:17Z- TNavPF::First Term of Map Covariance Matrix is 0.52 2024-11-06T04:51:17Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T04:51:17Z- TerrainNav::Resampling particle filter... 2024-11-06T04:51:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868676.02 sec. 2024-11-06T04:51:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:51:17Z- variances([0], [2], and psi_berg) = 0.567421 0.406150 0.000000 2024-11-06T04:51:17Z- publishEstimatesDecoder() - at 1730868677.20 sec 2024-11-06T04:51:17Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:51:17Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061659.66 594731.84 50.75 2024-11-06T04:51:17Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061659.33 594732.89 52.57 lat/lon: 36.70 -121.94 2024-11-06T04:51:17Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.41 0.64 0.00 2024-11-06T04:51:17Z- AHRS_M2 msg: 1730868677.59 epoch sec; seqNo:2221 2024-11-06T04:51:17Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.49 psi 2024-11-06T04:51:17Z- handleDepth()- Depth_Keller msg: 1730868677.66 epoch sec; seqNo:2377; depth 49.60 2024-11-06T04:51:17Z- handleDVL() - DVL msg epochMillisec: 1730868677.71 2024-11-06T04:51:17Z- handleDvl() - RDI_Pathfinder msg: 1730868677.71 sec; seqNo:879; #beams:4 2024-11-06T04:51:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 33.47 , 37.55 , 37.89 2024-11-06T04:51:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , -0.03 2024-11-06T04:51:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868677.73 epoch sec; seqNo:2377; 4061715.53 north; 594728.09 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:51:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868677.20 2024-11-06T04:51:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:17Z- waiting for 1730868681.20, time is 1730868677.73 2024-11-06T04:51:18Z- AHRS_M2 msg: 1730868678.01 epoch sec; seqNo:2222 2024-11-06T04:51:18Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T04:51:18Z- handleDepth()- Depth_Keller msg: 1730868678.01 epoch sec; seqNo:2378; depth 49.61 2024-11-06T04:51:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868678.03 epoch sec; seqNo:2378; 4061715.54 north; 594729.31 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:51:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868677.20 2024-11-06T04:51:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:18Z- waiting for 1730868681.20, time is 1730868678.03 2024-11-06T04:51:18Z- AHRS_M2 msg: 1730868678.40 epoch sec; seqNo:2223 2024-11-06T04:51:18Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:51:18Z- handleDepth()- Depth_Keller msg: 1730868678.40 epoch sec; seqNo:2379; depth 49.62 2024-11-06T04:51:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868678.42 epoch sec; seqNo:2379; 4061715.54 north; 594729.31 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:51:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868677.20 2024-11-06T04:51:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:18Z- waiting for 1730868681.20, time is 1730868678.42 2024-11-06T04:51:18Z- AHRS_M2 msg: 1730868678.81 epoch sec; seqNo:2224 2024-11-06T04:51:18Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:51:18Z- handleDepth()- Depth_Keller msg: 1730868678.82 epoch sec; seqNo:2380; depth 49.60 2024-11-06T04:51:18Z- handleDVL() - DVL msg epochMillisec: 1730868678.82 2024-11-06T04:51:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:51:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:51:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:51:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:51:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:51:18Z- handleDvl() - RDI_Pathfinder msg: 1730868678.82 sec; seqNo:880; #beams:0 2024-11-06T04:51:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:51:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , -0.03 2024-11-06T04:51:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868678.84 epoch sec; seqNo:2380; 4061715.54 north; 594729.31 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:51:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868677.20 2024-11-06T04:51:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:18Z- waiting for 1730868681.20, time is 1730868678.84 2024-11-06T04:51:19Z- AHRS_M2 msg: 1730868679.21 epoch sec; seqNo:2225 2024-11-06T04:51:19Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:51:19Z- handleDepth()- Depth_Keller msg: 1730868679.21 epoch sec; seqNo:2381; depth 49.60 2024-11-06T04:51:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868679.22 epoch sec; seqNo:2381; 4061715.54 north; 594729.31 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:51:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868677.20 2024-11-06T04:51:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:19Z- waiting for 1730868681.20, time is 1730868679.22 2024-11-06T04:51:19Z- AHRS_M2 msg: 1730868679.65 epoch sec; seqNo:2226 2024-11-06T04:51:19Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:51:19Z- handleDepth()- Depth_Keller msg: 1730868679.66 epoch sec; seqNo:2382; depth 49.60 2024-11-06T04:51:19Z- handleDVL() - DVL msg epochMillisec: 1730868679.68 2024-11-06T04:51:19Z- handleDvl() - RDI_Pathfinder msg: 1730868679.68 sec; seqNo:881; #beams:4 2024-11-06T04:51:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.80 , 33.30 , 37.38 , 38.23 2024-11-06T04:51:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.06 2024-11-06T04:51:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868679.70 epoch sec; seqNo:2382; 4061715.56 north; 594730.53 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:51:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868677.20 2024-11-06T04:51:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:19Z- waiting for 1730868681.20, time is 1730868679.70 2024-11-06T04:51:20Z- AHRS_M2 msg: 1730868680.01 epoch sec; seqNo:2227 2024-11-06T04:51:20Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:51:20Z- handleDepth()- Depth_Keller msg: 1730868680.02 epoch sec; seqNo:2383; depth 49.61 2024-11-06T04:51:20Z- handleDVL() - DVL msg epochMillisec: 1730868680.02 2024-11-06T04:51:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:51:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:51:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:51:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:51:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:51:20Z- handleDvl() - RDI_Pathfinder msg: 1730868680.02 sec; seqNo:882; #beams:0 2024-11-06T04:51:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:51:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.06 2024-11-06T04:51:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868680.05 epoch sec; seqNo:2383; 4061715.56 north; 594730.53 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:51:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868677.20 2024-11-06T04:51:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:20Z- waiting for 1730868681.20, time is 1730868680.05 2024-11-06T04:51:20Z- AHRS_M2 msg: 1730868680.42 epoch sec; seqNo:2228 2024-11-06T04:51:20Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:51:20Z- handleDepth()- Depth_Keller msg: 1730868680.42 epoch sec; seqNo:2384; depth 49.59 2024-11-06T04:51:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868680.43 epoch sec; seqNo:2384; 4061715.56 north; 594730.53 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:51:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868677.20 2024-11-06T04:51:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:20Z- waiting for 1730868681.20, time is 1730868680.43 2024-11-06T04:51:20Z- AHRS_M2 msg: 1730868680.83 epoch sec; seqNo:2229 2024-11-06T04:51:20Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:51:20Z- handleDepth()- Depth_Keller msg: 1730868680.86 epoch sec; seqNo:2385; depth 49.59 2024-11-06T04:51:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868680.88 epoch sec; seqNo:2385; 4061715.95 north; 594731.74 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:51:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868677.20 2024-11-06T04:51:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:21Z- waiting for 1730868681.20, time is 1730868680.88 2024-11-06T04:51:21Z- AHRS_M2 msg: 1730868681.23 epoch sec; seqNo:2230 2024-11-06T04:51:21Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.49 psi 2024-11-06T04:51:21Z- handleDepth()- Depth_Keller msg: 1730868681.23 epoch sec; seqNo:2386; depth 49.60 2024-11-06T04:51:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868681.24 epoch sec; seqNo:2386; 4061715.95 north; 594731.74 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:51:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868677.20 2024-11-06T04:51:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:21Z- waiting for 1730868681.20, time is 1730868681.24 2024-11-06T04:51:21Z- LcmTrn::updateTrn() >>>> pose time 1730868681.24, meas time 1730868680.02 <<<< 2024-11-06T04:51:21Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868681.24 2024-11-06T04:51:21Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:51:21Z- TerrainNav::motionUpdate() - currEstimate = (4061716, 594731.74, 49.60) 2024-11-06T04:51:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:51:21Z- TerrainNav:: SubcloudNIS is 0.54, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:51:21Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:51:21Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:51:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:51:21Z- variances([0], [2], and psi_berg) = 0.570861 0.407797 0.000000 2024-11-06T04:51:21Z- publishEstimatesDecoder() - at 1730868681.24 sec 2024-11-06T04:51:21Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:51:21Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061660.06 594735.48 52.07 2024-11-06T04:51:21Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061659.75 594736.55 52.56 lat/lon: 36.70 -121.94 2024-11-06T04:51:21Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.41 0.87 0.00 2024-11-06T04:51:21Z- AHRS_M2 msg: 1730868681.66 epoch sec; seqNo:2231 2024-11-06T04:51:21Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.49 psi 2024-11-06T04:51:21Z- handleDepth()- Depth_Keller msg: 1730868681.67 epoch sec; seqNo:2387; depth 49.60 2024-11-06T04:51:21Z- handleDVL() - DVL msg epochMillisec: 1730868681.69 2024-11-06T04:51:21Z- handleDvl() - RDI_Pathfinder msg: 1730868681.69 sec; seqNo:883; #beams:4 2024-11-06T04:51:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.97 , 33.30 , 37.21 , 38.23 2024-11-06T04:51:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.04 2024-11-06T04:51:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868681.71 epoch sec; seqNo:2387; 4061715.95 north; 594731.74 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:51:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868681.24 2024-11-06T04:51:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:21Z- waiting for 1730868685.24, time is 1730868681.71 2024-11-06T04:51:22Z- AHRS_M2 msg: 1730868682.04 epoch sec; seqNo:2232 2024-11-06T04:51:22Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.49 psi 2024-11-06T04:51:22Z- handleDepth()- Depth_Keller msg: 1730868682.04 epoch sec; seqNo:2388; depth 49.59 2024-11-06T04:51:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868682.05 epoch sec; seqNo:2388; 4061715.95 north; 594731.74 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:51:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868681.24 2024-11-06T04:51:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:22Z- waiting for 1730868685.24, time is 1730868682.05 2024-11-06T04:51:22Z- AHRS_M2 msg: 1730868682.44 epoch sec; seqNo:2233 2024-11-06T04:51:22Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.49 psi 2024-11-06T04:51:22Z- handleDepth()- Depth_Keller msg: 1730868682.45 epoch sec; seqNo:2389; depth 49.61 2024-11-06T04:51:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868682.46 epoch sec; seqNo:2389; 4061715.96 north; 594732.96 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:51:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868681.24 2024-11-06T04:51:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:22Z- waiting for 1730868685.24, time is 1730868682.46 2024-11-06T04:51:22Z- AHRS_M2 msg: 1730868682.85 epoch sec; seqNo:2234 2024-11-06T04:51:22Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.49 psi 2024-11-06T04:51:22Z- handleDepth()- Depth_Keller msg: 1730868682.85 epoch sec; seqNo:2390; depth 49.59 2024-11-06T04:51:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868682.87 epoch sec; seqNo:2390; 4061715.96 north; 594732.96 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:51:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868681.24 2024-11-06T04:51:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:23Z- waiting for 1730868685.24, time is 1730868682.87 2024-11-06T04:51:23Z- AHRS_M2 msg: 1730868683.25 epoch sec; seqNo:2235 2024-11-06T04:51:23Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.49 psi 2024-11-06T04:51:23Z- handleDepth()- Depth_Keller msg: 1730868683.25 epoch sec; seqNo:2391; depth 49.61 2024-11-06T04:51:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868683.27 epoch sec; seqNo:2391; 4061715.96 north; 594732.96 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:51:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868681.24 2024-11-06T04:51:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:23Z- waiting for 1730868685.24, time is 1730868683.27 2024-11-06T04:51:23Z- AHRS_M2 msg: 1730868683.67 epoch sec; seqNo:2236 2024-11-06T04:51:23Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.49 psi 2024-11-06T04:51:23Z- handleDepth()- Depth_Keller msg: 1730868683.67 epoch sec; seqNo:2392; depth 49.62 2024-11-06T04:51:23Z- handleDVL() - DVL msg epochMillisec: 1730868683.70 2024-11-06T04:51:23Z- handleDvl() - RDI_Pathfinder msg: 1730868683.70 sec; seqNo:884; #beams:4 2024-11-06T04:51:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.97 , 33.81 , 37.04 , 38.23 2024-11-06T04:51:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.06 , 0.01 2024-11-06T04:51:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868683.71 epoch sec; seqNo:2392; 4061715.98 north; 594734.18 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:51:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868681.24 2024-11-06T04:51:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:23Z- waiting for 1730868685.24, time is 1730868683.71 2024-11-06T04:51:24Z- AHRS_M2 msg: 1730868684.06 epoch sec; seqNo:2237 2024-11-06T04:51:24Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.48 psi 2024-11-06T04:51:24Z- handleDepth()- Depth_Keller msg: 1730868684.06 epoch sec; seqNo:2393; depth 49.62 2024-11-06T04:51:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868684.08 epoch sec; seqNo:2393; 4061716.35 north; 594734.18 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:51:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868681.24 2024-11-06T04:51:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:24Z- waiting for 1730868685.24, time is 1730868684.08 2024-11-06T04:51:24Z- AHRS_M2 msg: 1730868684.46 epoch sec; seqNo:2238 2024-11-06T04:51:24Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.48 psi 2024-11-06T04:51:24Z- handleDepth()- Depth_Keller msg: 1730868684.46 epoch sec; seqNo:2394; depth 49.62 2024-11-06T04:51:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868684.47 epoch sec; seqNo:2394; 4061716.35 north; 594734.18 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:51:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868681.24 2024-11-06T04:51:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:24Z- waiting for 1730868685.24, time is 1730868684.47 2024-11-06T04:51:24Z- AHRS_M2 msg: 1730868684.86 epoch sec; seqNo:2239 2024-11-06T04:51:24Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.48 psi 2024-11-06T04:51:24Z- handleDepth()- Depth_Keller msg: 1730868684.87 epoch sec; seqNo:2395; depth 49.63 2024-11-06T04:51:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868684.88 epoch sec; seqNo:2395; 4061716.35 north; 594734.18 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:51:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868681.24 2024-11-06T04:51:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:25Z- waiting for 1730868685.24, time is 1730868684.88 2024-11-06T04:51:25Z- AHRS_M2 msg: 1730868685.27 epoch sec; seqNo:2240 2024-11-06T04:51:25Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.48 psi 2024-11-06T04:51:25Z- handleDepth()- Depth_Keller msg: 1730868685.27 epoch sec; seqNo:2396; depth 49.64 2024-11-06T04:51:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868685.28 epoch sec; seqNo:2396; 4061716.37 north; 594735.40 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:51:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868681.24 2024-11-06T04:51:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:25Z- waiting for 1730868685.24, time is 1730868685.28 2024-11-06T04:51:25Z- LcmTrn::updateTrn() >>>> pose time 1730868685.28, meas time 1730868683.70 <<<< 2024-11-06T04:51:25Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868685.28 2024-11-06T04:51:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868683.70 sec, ping # 0; waiting for INS data more recent than 1730868681.24... 2024-11-06T04:51:25Z- TerrainNav::motionUpdate() - currEstimate = (4061716, 594735.40, 49.64) 2024-11-06T04:51:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:51:25Z- TerrainNav:: SubcloudNIS is 0.54, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:51:25Z- TNavPF::Projecting Measurements 2024-11-06T04:51:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:51:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:51:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:51:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:51:25Z- TNavFilter::4 good beams. 2024-11-06T04:51:25Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:51:25Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:51:25Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:51:25Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:51:25Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:51:25Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:51:25Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:51:25Z- TerrainMapOctree: (vehN, vehE ) = (4061716.20, 594733.97). 2024-11-06T04:51:25Z- TerrainMapOctree: Min Distance = 37.41. 2024-11-06T04:51:25Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:51:25Z- Forcing Subcloud Comparison 2024-11-06T04:51:25Z- Weighting particles with subcloud comparison 2024-11-06T04:51:25Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:25Z- meanExpectedMeasDiff: 2.307292 delta_rms_squared: 1.947072 alpha: 0.989833 2024-11-06T04:51:25Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:25Z- meanExpectedMeasDiff: -1.385792 delta_rms_squared: 2.008577 alpha: 0.990141 2024-11-06T04:51:25Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:25Z- meanExpectedMeasDiff: 0.292362 delta_rms_squared: 2.204794 alpha: 0.991010 2024-11-06T04:51:25Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:25Z- meanExpectedMeasDiff: -0.803578 delta_rms_squared: 1.558806 alpha: 0.987332 2024-11-06T04:51:25Z- Subcloud NIS: 0.528462 2024-11-06T04:51:25Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:51:25Z- TNavPF:: sumWeights = 0.224672 2024-11-06T04:51:25Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:51:25Z- TNavPF::First Term of Map Covariance Matrix is 0.87 2024-11-06T04:51:25Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T04:51:25Z- TerrainNav::Resampling particle filter... 2024-11-06T04:51:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868683.70 sec. 2024-11-06T04:51:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:51:25Z- variances([0], [2], and psi_berg) = 0.555329 0.403947 0.000000 2024-11-06T04:51:25Z- publishEstimatesDecoder() - at 1730868685.28 sec 2024-11-06T04:51:25Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:51:25Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061660.42 594739.30 53.29 2024-11-06T04:51:25Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061660.28 594740.26 52.57 lat/lon: 36.70 -121.94 2024-11-06T04:51:25Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.56 0.40 0.67 0.00 2024-11-06T04:51:25Z- AHRS_M2 msg: 1730868685.67 epoch sec; seqNo:2241 2024-11-06T04:51:25Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.48 psi 2024-11-06T04:51:25Z- handleDepth()- Depth_Keller msg: 1730868685.69 epoch sec; seqNo:2397; depth 49.67 2024-11-06T04:51:25Z- handleDVL() - DVL msg epochMillisec: 1730868685.76 2024-11-06T04:51:25Z- handleDvl() - RDI_Pathfinder msg: 1730868685.76 sec; seqNo:885; #beams:4 2024-11-06T04:51:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 33.81 , 36.87 , 38.40 2024-11-06T04:51:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.04 , 0.04 2024-11-06T04:51:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868685.79 epoch sec; seqNo:2397; 4061716.37 north; 594735.40 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:51:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868685.28 2024-11-06T04:51:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:25Z- waiting for 1730868689.28, time is 1730868685.79 2024-11-06T04:51:26Z- AHRS_M2 msg: 1730868686.08 epoch sec; seqNo:2242 2024-11-06T04:51:26Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.48 psi 2024-11-06T04:51:26Z- handleDepth()- Depth_Keller msg: 1730868686.08 epoch sec; seqNo:2398; depth 49.68 2024-11-06T04:51:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868686.10 epoch sec; seqNo:2398; 4061716.37 north; 594735.40 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T04:51:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868685.28 2024-11-06T04:51:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:26Z- waiting for 1730868689.28, time is 1730868686.10 2024-11-06T04:51:26Z- AHRS_M2 msg: 1730868686.48 epoch sec; seqNo:2243 2024-11-06T04:51:26Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.48 psi 2024-11-06T04:51:26Z- handleDepth()- Depth_Keller msg: 1730868686.49 epoch sec; seqNo:2399; depth 49.65 2024-11-06T04:51:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868686.53 epoch sec; seqNo:2399; 4061716.37 north; 594735.40 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:51:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868685.28 2024-11-06T04:51:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:26Z- waiting for 1730868689.28, time is 1730868686.53 2024-11-06T04:51:26Z- AHRS_M2 msg: 1730868686.88 epoch sec; seqNo:2244 2024-11-06T04:51:26Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.48 psi 2024-11-06T04:51:26Z- handleDepth()- Depth_Keller msg: 1730868686.89 epoch sec; seqNo:2400; depth 49.67 2024-11-06T04:51:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868686.90 epoch sec; seqNo:2400; 4061716.38 north; 594736.62 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:51:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868685.28 2024-11-06T04:51:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:27Z- waiting for 1730868689.28, time is 1730868686.90 2024-11-06T04:51:27Z- AHRS_M2 msg: 1730868687.29 epoch sec; seqNo:2245 2024-11-06T04:51:27Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.48 psi 2024-11-06T04:51:27Z- handleDepth()- Depth_Keller msg: 1730868687.29 epoch sec; seqNo:2401; depth 49.69 2024-11-06T04:51:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868687.30 epoch sec; seqNo:2401; 4061716.76 north; 594736.62 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T04:51:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868685.28 2024-11-06T04:51:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:27Z- waiting for 1730868689.28, time is 1730868687.30 2024-11-06T04:51:27Z- AHRS_M2 msg: 1730868687.69 epoch sec; seqNo:2246 2024-11-06T04:51:27Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.48 psi 2024-11-06T04:51:27Z- handleDepth()- Depth_Keller msg: 1730868687.70 epoch sec; seqNo:2402; depth 49.70 2024-11-06T04:51:27Z- handleDVL() - DVL msg epochMillisec: 1730868687.74 2024-11-06T04:51:27Z- handleDvl() - RDI_Pathfinder msg: 1730868687.74 sec; seqNo:886; #beams:4 2024-11-06T04:51:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 34.15 , 36.70 , 38.40 2024-11-06T04:51:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , 0.02 2024-11-06T04:51:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868687.75 epoch sec; seqNo:2402; 4061716.76 north; 594736.62 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T04:51:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868685.28 2024-11-06T04:51:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:27Z- waiting for 1730868689.28, time is 1730868687.75 2024-11-06T04:51:28Z- AHRS_M2 msg: 1730868688.10 epoch sec; seqNo:2247 2024-11-06T04:51:28Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:51:28Z- handleDepth()- Depth_Keller msg: 1730868688.10 epoch sec; seqNo:2403; depth 49.71 2024-11-06T04:51:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868688.14 epoch sec; seqNo:2403; 4061716.77 north; 594737.84 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T04:51:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868685.28 2024-11-06T04:51:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:28Z- waiting for 1730868689.28, time is 1730868688.14 2024-11-06T04:51:28Z- handleDepth()- Depth_Keller msg: 1730868688.52 epoch sec; seqNo:2404; depth 49.72 2024-11-06T04:51:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868688.54 epoch sec; seqNo:2404; 4061716.77 north; 594737.84 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:51:28Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:51:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868685.28 2024-11-06T04:51:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:28Z- waiting for 1730868689.28, time is 1730868688.54 2024-11-06T04:51:28Z- AHRS_M2 msg: 1730868688.91 epoch sec; seqNo:2248 2024-11-06T04:51:28Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:51:28Z- handleDepth()- Depth_Keller msg: 1730868688.92 epoch sec; seqNo:2405; depth 49.73 2024-11-06T04:51:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868688.93 epoch sec; seqNo:2405; 4061716.77 north; 594737.84 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T04:51:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868685.28 2024-11-06T04:51:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:29Z- waiting for 1730868689.28, time is 1730868688.93 2024-11-06T04:51:29Z- AHRS_M2 msg: 1730868689.31 epoch sec; seqNo:2249 2024-11-06T04:51:29Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:51:29Z- handleDepth()- Depth_Keller msg: 1730868689.32 epoch sec; seqNo:2406; depth 49.74 2024-11-06T04:51:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868689.34 epoch sec; seqNo:2406; 4061716.79 north; 594739.06 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T04:51:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868685.28 2024-11-06T04:51:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:29Z- waiting for 1730868689.28, time is 1730868689.34 2024-11-06T04:51:29Z- LcmTrn::updateTrn() >>>> pose time 1730868689.34, meas time 1730868687.74 <<<< 2024-11-06T04:51:29Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868689.34 2024-11-06T04:51:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868687.74 sec, ping # 0; waiting for INS data more recent than 1730868685.28... 2024-11-06T04:51:29Z- TerrainNav::motionUpdate() - currEstimate = (4061717, 594739.06, 49.74) 2024-11-06T04:51:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:51:29Z- TerrainNav:: SubcloudNIS is 0.53, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:51:29Z- TNavPF::Projecting Measurements 2024-11-06T04:51:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:51:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:51:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:51:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:51:29Z- TNavFilter::4 good beams. 2024-11-06T04:51:29Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:51:29Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:51:29Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:51:29Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:51:29Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:51:29Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:51:29Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:51:29Z- TerrainMapOctree: (vehN, vehE ) = (4061716.62, 594737.61). 2024-11-06T04:51:29Z- TerrainMapOctree: Min Distance = 35.60. 2024-11-06T04:51:29Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:51:29Z- Forcing Subcloud Comparison 2024-11-06T04:51:29Z- Weighting particles with subcloud comparison 2024-11-06T04:51:29Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:29Z- meanExpectedMeasDiff: 0.379247 delta_rms_squared: 2.864332 alpha: 0.993066 2024-11-06T04:51:29Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:29Z- meanExpectedMeasDiff: 0.382828 delta_rms_squared: 1.645155 alpha: 0.987989 2024-11-06T04:51:29Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:29Z- meanExpectedMeasDiff: -0.600898 delta_rms_squared: 2.133176 alpha: 0.990711 2024-11-06T04:51:29Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:29Z- meanExpectedMeasDiff: -0.753549 delta_rms_squared: 1.283922 alpha: 0.984662 2024-11-06T04:51:29Z- Subcloud NIS: 0.496308 2024-11-06T04:51:29Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:51:29Z- TNavPF:: sumWeights = 0.472357 2024-11-06T04:51:29Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:51:29Z- TNavPF::First Term of Map Covariance Matrix is 0.58 2024-11-06T04:51:29Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.03 2024-11-06T04:51:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868687.74 sec. 2024-11-06T04:51:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:51:29Z- variances([0], [2], and psi_berg) = 0.559008 0.413397 0.000000 2024-11-06T04:51:29Z- publishEstimatesDecoder() - at 1730868689.34 sec 2024-11-06T04:51:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:51:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061661.72 594742.99 52.61 2024-11-06T04:51:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061660.68 594743.89 52.75 lat/lon: 36.70 -121.94 2024-11-06T04:51:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.56 0.41 0.61 0.00 2024-11-06T04:51:29Z- AHRS_M2 msg: 1730868689.71 epoch sec; seqNo:2250 2024-11-06T04:51:29Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:51:29Z- handleDepth()- Depth_Keller msg: 1730868689.72 epoch sec; seqNo:2407; depth 49.74 2024-11-06T04:51:29Z- handleDVL() - DVL msg epochMillisec: 1730868689.76 2024-11-06T04:51:29Z- handleDvl() - RDI_Pathfinder msg: 1730868689.76 sec; seqNo:887; #beams:4 2024-11-06T04:51:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.80 , 34.15 , 36.70 , 38.40 2024-11-06T04:51:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , -0.02 2024-11-06T04:51:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868689.78 epoch sec; seqNo:2407; 4061716.79 north; 594739.06 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T04:51:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868689.34 2024-11-06T04:51:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:29Z- waiting for 1730868693.34, time is 1730868689.78 2024-11-06T04:51:30Z- AHRS_M2 msg: 1730868690.11 epoch sec; seqNo:2251 2024-11-06T04:51:30Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.49 psi 2024-11-06T04:51:30Z- handleDepth()- Depth_Keller msg: 1730868690.12 epoch sec; seqNo:2408; depth 49.76 2024-11-06T04:51:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868690.13 epoch sec; seqNo:2408; 4061716.79 north; 594739.06 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T04:51:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868689.34 2024-11-06T04:51:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:30Z- waiting for 1730868693.34, time is 1730868690.13 2024-11-06T04:51:30Z- AHRS_M2 msg: 1730868690.52 epoch sec; seqNo:2252 2024-11-06T04:51:30Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.49 psi 2024-11-06T04:51:30Z- handleDepth()- Depth_Keller msg: 1730868690.53 epoch sec; seqNo:2409; depth 49.74 2024-11-06T04:51:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868690.54 epoch sec; seqNo:2409; 4061716.79 north; 594739.06 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T04:51:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868689.34 2024-11-06T04:51:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:30Z- waiting for 1730868693.34, time is 1730868690.54 2024-11-06T04:51:30Z- AHRS_M2 msg: 1730868690.92 epoch sec; seqNo:2253 2024-11-06T04:51:30Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.49 psi 2024-11-06T04:51:30Z- handleDepth()- Depth_Keller msg: 1730868690.94 epoch sec; seqNo:2410; depth 49.76 2024-11-06T04:51:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868690.95 epoch sec; seqNo:2410; 4061716.80 north; 594740.28 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T04:51:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868689.34 2024-11-06T04:51:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:31Z- waiting for 1730868693.34, time is 1730868690.95 2024-11-06T04:51:31Z- AHRS_M2 msg: 1730868691.33 epoch sec; seqNo:2254 2024-11-06T04:51:31Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.49 psi 2024-11-06T04:51:31Z- handleDepth()- Depth_Keller msg: 1730868691.33 epoch sec; seqNo:2411; depth 49.77 2024-11-06T04:51:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868691.35 epoch sec; seqNo:2411; 4061717.18 north; 594740.27 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T04:51:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868689.34 2024-11-06T04:51:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:31Z- waiting for 1730868693.34, time is 1730868691.35 2024-11-06T04:51:31Z- AHRS_M2 msg: 1730868691.73 epoch sec; seqNo:2255 2024-11-06T04:51:31Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.49 psi 2024-11-06T04:51:31Z- handleDepth()- Depth_Keller msg: 1730868691.74 epoch sec; seqNo:2412; depth 49.75 2024-11-06T04:51:31Z- handleDVL() - DVL msg epochMillisec: 1730868691.77 2024-11-06T04:51:31Z- handleDvl() - RDI_Pathfinder msg: 1730868691.77 sec; seqNo:888; #beams:4 2024-11-06T04:51:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 33.81 , 36.53 , 38.23 2024-11-06T04:51:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , -0.03 2024-11-06T04:51:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868691.79 epoch sec; seqNo:2412; 4061717.18 north; 594740.27 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:51:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868689.34 2024-11-06T04:51:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:31Z- waiting for 1730868693.34, time is 1730868691.79 2024-11-06T04:51:32Z- AHRS_M2 msg: 1730868692.14 epoch sec; seqNo:2256 2024-11-06T04:51:32Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.49 psi 2024-11-06T04:51:32Z- handleDepth()- Depth_Keller msg: 1730868692.14 epoch sec; seqNo:2413; depth 49.78 2024-11-06T04:51:32Z- handleDVL() - DVL msg epochMillisec: 1730868692.14 2024-11-06T04:51:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:51:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:51:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:51:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:51:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:51:32Z- handleDvl() - RDI_Pathfinder msg: 1730868692.14 sec; seqNo:889; #beams:0 2024-11-06T04:51:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:51:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , -0.03 2024-11-06T04:51:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868692.16 epoch sec; seqNo:2413; 4061717.19 north; 594741.49 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:51:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868689.34 2024-11-06T04:51:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:32Z- waiting for 1730868693.34, time is 1730868692.16 2024-11-06T04:51:32Z- AHRS_M2 msg: 1730868692.54 epoch sec; seqNo:2257 2024-11-06T04:51:32Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.49 psi 2024-11-06T04:51:32Z- handleDepth()- Depth_Keller msg: 1730868692.54 epoch sec; seqNo:2414; depth 49.78 2024-11-06T04:51:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868692.55 epoch sec; seqNo:2414; 4061717.19 north; 594741.49 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:51:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868689.34 2024-11-06T04:51:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:32Z- waiting for 1730868693.34, time is 1730868692.55 2024-11-06T04:51:32Z- AHRS_M2 msg: 1730868692.94 epoch sec; seqNo:2258 2024-11-06T04:51:32Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.49 psi 2024-11-06T04:51:32Z- handleDepth()- Depth_Keller msg: 1730868692.95 epoch sec; seqNo:2415; depth 49.78 2024-11-06T04:51:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868692.96 epoch sec; seqNo:2415; 4061717.19 north; 594741.49 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:51:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868689.34 2024-11-06T04:51:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:33Z- waiting for 1730868693.34, time is 1730868692.96 2024-11-06T04:51:33Z- AHRS_M2 msg: 1730868693.36 epoch sec; seqNo:2259 2024-11-06T04:51:33Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.49 psi 2024-11-06T04:51:33Z- handleDepth()- Depth_Keller msg: 1730868693.37 epoch sec; seqNo:2416; depth 49.79 2024-11-06T04:51:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868693.38 epoch sec; seqNo:2416; 4061717.19 north; 594741.49 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:51:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868689.34 2024-11-06T04:51:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:33Z- waiting for 1730868693.34, time is 1730868693.38 2024-11-06T04:51:33Z- LcmTrn::updateTrn() >>>> pose time 1730868693.38, meas time 1730868692.14 <<<< 2024-11-06T04:51:33Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868693.38 2024-11-06T04:51:33Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:51:33Z- TerrainNav::motionUpdate() - currEstimate = (4061717, 594741.49, 49.79) 2024-11-06T04:51:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:51:33Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:51:33Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:51:33Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:51:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:51:33Z- variances([0], [2], and psi_berg) = 0.559826 0.414454 0.000000 2024-11-06T04:51:33Z- publishEstimatesDecoder() - at 1730868693.38 sec 2024-11-06T04:51:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:51:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061662.08 594745.41 52.10 2024-11-06T04:51:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061661.09 594746.33 52.79 lat/lon: 36.70 -121.94 2024-11-06T04:51:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.56 0.41 0.87 0.00 2024-11-06T04:51:33Z- AHRS_M2 msg: 1730868693.75 epoch sec; seqNo:2260 2024-11-06T04:51:33Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.49 psi 2024-11-06T04:51:33Z- handleDepth()- Depth_Keller msg: 1730868693.76 epoch sec; seqNo:2417; depth 49.79 2024-11-06T04:51:33Z- handleDVL() - DVL msg epochMillisec: 1730868693.80 2024-11-06T04:51:33Z- handleDvl() - RDI_Pathfinder msg: 1730868693.80 sec; seqNo:890; #beams:4 2024-11-06T04:51:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.97 , 33.64 , 36.19 , 38.23 2024-11-06T04:51:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , -0.02 2024-11-06T04:51:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868693.83 epoch sec; seqNo:2417; 4061717.21 north; 594742.71 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:51:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868693.38 2024-11-06T04:51:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:33Z- waiting for 1730868697.38, time is 1730868693.83 2024-11-06T04:51:34Z- AHRS_M2 msg: 1730868694.15 epoch sec; seqNo:2261 2024-11-06T04:51:34Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:51:34Z- handleDepth()- Depth_Keller msg: 1730868694.16 epoch sec; seqNo:2418; depth 49.81 2024-11-06T04:51:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868694.17 epoch sec; seqNo:2418; 4061717.21 north; 594742.71 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:51:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868693.38 2024-11-06T04:51:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:34Z- waiting for 1730868697.38, time is 1730868694.17 2024-11-06T04:51:34Z- AHRS_M2 msg: 1730868694.56 epoch sec; seqNo:2262 2024-11-06T04:51:34Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:51:34Z- handleDepth()- Depth_Keller msg: 1730868694.57 epoch sec; seqNo:2419; depth 49.82 2024-11-06T04:51:34Z- handleDVL() - DVL msg epochMillisec: 1730868694.57 2024-11-06T04:51:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:51:34Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:51:34Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:51:34Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:51:34Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:51:34Z- handleDvl() - RDI_Pathfinder msg: 1730868694.57 sec; seqNo:891; #beams:0 2024-11-06T04:51:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:51:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , -0.02 2024-11-06T04:51:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868694.58 epoch sec; seqNo:2419; 4061717.21 north; 594742.71 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:51:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868693.38 2024-11-06T04:51:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:34Z- waiting for 1730868697.38, time is 1730868694.58 2024-11-06T04:51:34Z- AHRS_M2 msg: 1730868694.97 epoch sec; seqNo:2263 2024-11-06T04:51:34Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:51:34Z- handleDepth()- Depth_Keller msg: 1730868694.97 epoch sec; seqNo:2420; depth 49.82 2024-11-06T04:51:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868694.98 epoch sec; seqNo:2420; 4061717.22 north; 594743.93 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:51:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868693.38 2024-11-06T04:51:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:35Z- waiting for 1730868697.38, time is 1730868694.98 2024-11-06T04:51:35Z- AHRS_M2 msg: 1730868695.37 epoch sec; seqNo:2264 2024-11-06T04:51:35Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:51:35Z- handleDepth()- Depth_Keller msg: 1730868695.37 epoch sec; seqNo:2421; depth 49.83 2024-11-06T04:51:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868695.38 epoch sec; seqNo:2421; 4061717.60 north; 594743.93 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:51:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868693.38 2024-11-06T04:51:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:35Z- waiting for 1730868697.38, time is 1730868695.38 2024-11-06T04:51:35Z- AHRS_M2 msg: 1730868695.77 epoch sec; seqNo:2265 2024-11-06T04:51:35Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:51:35Z- handleDepth()- Depth_Keller msg: 1730868695.78 epoch sec; seqNo:2422; depth 49.84 2024-11-06T04:51:35Z- handleDVL() - DVL msg epochMillisec: 1730868695.80 2024-11-06T04:51:35Z- handleDvl() - RDI_Pathfinder msg: 1730868695.80 sec; seqNo:892; #beams:4 2024-11-06T04:51:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 33.81 , 35.85 , 38.40 2024-11-06T04:51:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.01 2024-11-06T04:51:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868695.83 epoch sec; seqNo:2422; 4061717.60 north; 594743.93 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T04:51:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868693.38 2024-11-06T04:51:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:35Z- waiting for 1730868697.38, time is 1730868695.83 2024-11-06T04:51:36Z- AHRS_M2 msg: 1730868696.18 epoch sec; seqNo:2266 2024-11-06T04:51:36Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:51:36Z- handleDepth()- Depth_Keller msg: 1730868696.19 epoch sec; seqNo:2423; depth 49.85 2024-11-06T04:51:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868696.20 epoch sec; seqNo:2423; 4061717.60 north; 594743.93 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:51:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868693.38 2024-11-06T04:51:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:36Z- waiting for 1730868697.38, time is 1730868696.20 2024-11-06T04:51:36Z- AHRS_M2 msg: 1730868696.58 epoch sec; seqNo:2267 2024-11-06T04:51:36Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:51:36Z- handleDepth()- Depth_Keller msg: 1730868696.59 epoch sec; seqNo:2424; depth 49.86 2024-11-06T04:51:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868696.59 epoch sec; seqNo:2424; 4061717.61 north; 594745.15 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:51:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868693.38 2024-11-06T04:51:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:36Z- waiting for 1730868697.38, time is 1730868696.59 2024-11-06T04:51:36Z- AHRS_M2 msg: 1730868696.98 epoch sec; seqNo:2268 2024-11-06T04:51:36Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:51:36Z- handleDepth()- Depth_Keller msg: 1730868696.99 epoch sec; seqNo:2425; depth 49.87 2024-11-06T04:51:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868697.01 epoch sec; seqNo:2425; 4061717.61 north; 594745.15 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:51:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868693.38 2024-11-06T04:51:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:37Z- waiting for 1730868697.38, time is 1730868697.01 2024-11-06T04:51:37Z- AHRS_M2 msg: 1730868697.39 epoch sec; seqNo:2269 2024-11-06T04:51:37Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:51:37Z- handleDepth()- Depth_Keller msg: 1730868697.39 epoch sec; seqNo:2426; depth 49.90 2024-11-06T04:51:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868697.40 epoch sec; seqNo:2426; 4061717.61 north; 594745.15 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:51:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868693.38 2024-11-06T04:51:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:37Z- waiting for 1730868697.38, time is 1730868697.40 2024-11-06T04:51:37Z- LcmTrn::updateTrn() >>>> pose time 1730868697.40, meas time 1730868695.80 <<<< 2024-11-06T04:51:37Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868697.40 2024-11-06T04:51:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868695.80 sec, ping # 0; waiting for INS data more recent than 1730868693.38... 2024-11-06T04:51:37Z- TerrainNav::motionUpdate() - currEstimate = (4061718, 594745.15, 49.90) 2024-11-06T04:51:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:51:37Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:51:37Z- TNavPF::Projecting Measurements 2024-11-06T04:51:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:51:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:51:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:51:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:51:37Z- TNavFilter::4 good beams. 2024-11-06T04:51:37Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:51:37Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:51:37Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:51:37Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:51:37Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:51:37Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:51:37Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:51:37Z- TerrainMapOctree: (vehN, vehE ) = (4061717.44, 594743.69). 2024-11-06T04:51:37Z- TerrainMapOctree: Min Distance = 33.16. 2024-11-06T04:51:37Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:51:37Z- Forcing Subcloud Comparison 2024-11-06T04:51:37Z- Weighting particles with subcloud comparison 2024-11-06T04:51:37Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:37Z- meanExpectedMeasDiff: -1.812321 delta_rms_squared: 1.869082 alpha: 0.989413 2024-11-06T04:51:37Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:37Z- meanExpectedMeasDiff: 0.860509 delta_rms_squared: 1.424838 alpha: 0.986158 2024-11-06T04:51:37Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:37Z- meanExpectedMeasDiff: -0.584232 delta_rms_squared: 1.483220 alpha: 0.986695 2024-11-06T04:51:37Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:37Z- meanExpectedMeasDiff: 0.506775 delta_rms_squared: 2.235315 alpha: 0.991132 2024-11-06T04:51:37Z- Subcloud NIS: 0.470471 2024-11-06T04:51:37Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:51:37Z- TNavPF:: sumWeights = 0.356993 2024-11-06T04:51:37Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:51:37Z- TNavPF::First Term of Map Covariance Matrix is 0.66 2024-11-06T04:51:37Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T04:51:37Z- TerrainNav::Resampling particle filter... 2024-11-06T04:51:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868695.80 sec. 2024-11-06T04:51:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:51:37Z- variances([0], [2], and psi_berg) = 0.542588 0.390205 0.000000 2024-11-06T04:51:37Z- publishEstimatesDecoder() - at 1730868697.40 sec 2024-11-06T04:51:37Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:51:37Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061661.68 594748.95 53.48 2024-11-06T04:51:37Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061661.49 594749.95 53.03 lat/lon: 36.70 -121.94 2024-11-06T04:51:37Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.54 0.39 0.68 0.00 2024-11-06T04:51:37Z- AHRS_M2 msg: 1730868697.80 epoch sec; seqNo:2270 2024-11-06T04:51:37Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:51:37Z- handleDepth()- Depth_Keller msg: 1730868697.81 epoch sec; seqNo:2427; depth 49.90 2024-11-06T04:51:37Z- handleDVL() - DVL msg epochMillisec: 1730868697.84 2024-11-06T04:51:37Z- handleDvl() - RDI_Pathfinder msg: 1730868697.84 sec; seqNo:893; #beams:4 2024-11-06T04:51:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.29 , 33.64 , 35.00 , 38.23 2024-11-06T04:51:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.01 2024-11-06T04:51:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868697.87 epoch sec; seqNo:2427; 4061717.63 north; 594746.37 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:51:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868697.40 2024-11-06T04:51:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:37Z- waiting for 1730868701.40, time is 1730868697.87 2024-11-06T04:51:38Z- AHRS_M2 msg: 1730868698.20 epoch sec; seqNo:2271 2024-11-06T04:51:38Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:51:38Z- handleDepth()- Depth_Keller msg: 1730868698.20 epoch sec; seqNo:2428; depth 49.91 2024-11-06T04:51:38Z- handleDVL() - DVL msg epochMillisec: 1730868698.21 2024-11-06T04:51:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:51:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:51:38Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:51:38Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:51:38Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:51:38Z- handleDvl() - RDI_Pathfinder msg: 1730868698.21 sec; seqNo:894; #beams:0 2024-11-06T04:51:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:51:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.01 2024-11-06T04:51:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868698.23 epoch sec; seqNo:2428; 4061717.63 north; 594746.37 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:51:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868697.40 2024-11-06T04:51:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:38Z- waiting for 1730868701.40, time is 1730868698.23 2024-11-06T04:51:38Z- AHRS_M2 msg: 1730868698.60 epoch sec; seqNo:2272 2024-11-06T04:51:38Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:51:38Z- handleDepth()- Depth_Keller msg: 1730868698.61 epoch sec; seqNo:2429; depth 49.94 2024-11-06T04:51:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868698.62 epoch sec; seqNo:2429; 4061717.63 north; 594746.37 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T04:51:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868697.40 2024-11-06T04:51:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:38Z- waiting for 1730868701.40, time is 1730868698.62 2024-11-06T04:51:39Z- AHRS_M2 msg: 1730868699.01 epoch sec; seqNo:2273 2024-11-06T04:51:39Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:51:39Z- handleDepth()- Depth_Keller msg: 1730868699.01 epoch sec; seqNo:2430; depth 49.95 2024-11-06T04:51:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868699.02 epoch sec; seqNo:2430; 4061718.00 north; 594746.37 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T04:51:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868697.40 2024-11-06T04:51:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:39Z- waiting for 1730868701.40, time is 1730868699.02 2024-11-06T04:51:39Z- AHRS_M2 msg: 1730868699.42 epoch sec; seqNo:2274 2024-11-06T04:51:39Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:51:39Z- handleDepth()- Depth_Keller msg: 1730868699.43 epoch sec; seqNo:2431; depth 49.97 2024-11-06T04:51:39Z- handleDVL() - DVL msg epochMillisec: 1730868699.43 2024-11-06T04:51:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:51:39Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:51:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:51:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:51:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:51:39Z- handleDvl() - RDI_Pathfinder msg: 1730868699.43 sec; seqNo:895; #beams:0 2024-11-06T04:51:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:51:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , 0.01 2024-11-06T04:51:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868699.44 epoch sec; seqNo:2431; 4061718.02 north; 594747.59 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T04:51:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868697.40 2024-11-06T04:51:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:39Z- waiting for 1730868701.40, time is 1730868699.44 2024-11-06T04:51:39Z- AHRS_M2 msg: 1730868699.82 epoch sec; seqNo:2275 2024-11-06T04:51:39Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:51:39Z- handleDepth()- Depth_Keller msg: 1730868699.84 epoch sec; seqNo:2432; depth 49.98 2024-11-06T04:51:39Z- handleDVL() - DVL msg epochMillisec: 1730868699.86 2024-11-06T04:51:39Z- handleDvl() - RDI_Pathfinder msg: 1730868699.86 sec; seqNo:896; #beams:4 2024-11-06T04:51:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.28 , 33.64 , 35.00 , 38.56 2024-11-06T04:51:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.02 2024-11-06T04:51:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868699.87 epoch sec; seqNo:2432; 4061718.02 north; 594747.59 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T04:51:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868697.40 2024-11-06T04:51:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:39Z- waiting for 1730868701.40, time is 1730868699.87 2024-11-06T04:51:40Z- AHRS_M2 msg: 1730868700.21 epoch sec; seqNo:2276 2024-11-06T04:51:40Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:51:40Z- handleDepth()- Depth_Keller msg: 1730868700.22 epoch sec; seqNo:2433; depth 50.00 2024-11-06T04:51:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868700.24 epoch sec; seqNo:2433; 4061718.02 north; 594747.59 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T04:51:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868697.40 2024-11-06T04:51:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:40Z- waiting for 1730868701.40, time is 1730868700.24 2024-11-06T04:51:40Z- AHRS_M2 msg: 1730868700.62 epoch sec; seqNo:2277 2024-11-06T04:51:40Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.49 psi 2024-11-06T04:51:40Z- handleDepth()- Depth_Keller msg: 1730868700.63 epoch sec; seqNo:2434; depth 50.02 2024-11-06T04:51:40Z- handleDVL() - DVL msg epochMillisec: 1730868700.63 2024-11-06T04:51:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:51:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:51:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:51:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:51:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:51:40Z- handleDvl() - RDI_Pathfinder msg: 1730868700.63 sec; seqNo:897; #beams:0 2024-11-06T04:51:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:51:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , 0.02 2024-11-06T04:51:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868700.64 epoch sec; seqNo:2434; 4061718.02 north; 594747.59 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:51:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868697.40 2024-11-06T04:51:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:40Z- waiting for 1730868701.40, time is 1730868700.64 2024-11-06T04:51:41Z- AHRS_M2 msg: 1730868701.03 epoch sec; seqNo:2278 2024-11-06T04:51:41Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T04:51:41Z- handleDepth()- Depth_Keller msg: 1730868701.04 epoch sec; seqNo:2435; depth 50.03 2024-11-06T04:51:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868701.05 epoch sec; seqNo:2435; 4061718.03 north; 594748.81 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T04:51:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868697.40 2024-11-06T04:51:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:41Z- waiting for 1730868701.40, time is 1730868701.05 2024-11-06T04:51:41Z- AHRS_M2 msg: 1730868701.43 epoch sec; seqNo:2279 2024-11-06T04:51:41Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T04:51:41Z- handleDepth()- Depth_Keller msg: 1730868701.43 epoch sec; seqNo:2436; depth 50.05 2024-11-06T04:51:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868701.44 epoch sec; seqNo:2436; 4061718.03 north; 594748.81 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T04:51:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868697.40 2024-11-06T04:51:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:41Z- waiting for 1730868701.40, time is 1730868701.44 2024-11-06T04:51:41Z- LcmTrn::updateTrn() >>>> pose time 1730868701.44, meas time 1730868700.63 <<<< 2024-11-06T04:51:41Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868701.44 2024-11-06T04:51:41Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:51:41Z- TerrainNav::motionUpdate() - currEstimate = (4061718, 594748.81, 50.05) 2024-11-06T04:51:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:51:41Z- TerrainNav:: SubcloudNIS is 0.47, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:51:41Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:51:41Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:51:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:51:41Z- variances([0], [2], and psi_berg) = 0.544513 0.392302 0.000000 2024-11-06T04:51:41Z- publishEstimatesDecoder() - at 1730868701.44 sec 2024-11-06T04:51:41Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:51:41Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061662.18 594752.58 53.78 2024-11-06T04:51:41Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061661.91 594753.60 53.17 lat/lon: 36.70 -121.94 2024-11-06T04:51:41Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.54 0.39 0.92 0.00 2024-11-06T04:51:41Z- AHRS_M2 msg: 1730868701.84 epoch sec; seqNo:2280 2024-11-06T04:51:41Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T04:51:41Z- handleDepth()- Depth_Keller msg: 1730868701.84 epoch sec; seqNo:2437; depth 50.06 2024-11-06T04:51:41Z- handleDVL() - DVL msg epochMillisec: 1730868701.86 2024-11-06T04:51:41Z- handleDvl() - RDI_Pathfinder msg: 1730868701.86 sec; seqNo:898; #beams:4 2024-11-06T04:51:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.10 , 33.47 , 34.49 , 38.40 2024-11-06T04:51:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.04 , -0.00 2024-11-06T04:51:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868701.89 epoch sec; seqNo:2437; 4061718.03 north; 594748.81 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T04:51:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868701.44 2024-11-06T04:51:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:41Z- waiting for 1730868705.44, time is 1730868701.89 2024-11-06T04:51:42Z- AHRS_M2 msg: 1730868702.24 epoch sec; seqNo:2281 2024-11-06T04:51:42Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T04:51:42Z- handleDepth()- Depth_Keller msg: 1730868702.24 epoch sec; seqNo:2438; depth 50.09 2024-11-06T04:51:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868702.26 epoch sec; seqNo:2438; 4061718.05 north; 594750.03 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T04:51:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868701.44 2024-11-06T04:51:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:42Z- waiting for 1730868705.44, time is 1730868702.26 2024-11-06T04:51:42Z- AHRS_M2 msg: 1730868702.64 epoch sec; seqNo:2282 2024-11-06T04:51:42Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T04:51:42Z- handleDepth()- Depth_Keller msg: 1730868702.64 epoch sec; seqNo:2439; depth 50.09 2024-11-06T04:51:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868702.65 epoch sec; seqNo:2439; 4061718.05 north; 594750.03 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T04:51:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868701.44 2024-11-06T04:51:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:42Z- waiting for 1730868705.44, time is 1730868702.65 2024-11-06T04:51:43Z- AHRS_M2 msg: 1730868703.05 epoch sec; seqNo:2283 2024-11-06T04:51:43Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T04:51:43Z- handleDepth()- Depth_Keller msg: 1730868703.05 epoch sec; seqNo:2440; depth 50.10 2024-11-06T04:51:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868703.07 epoch sec; seqNo:2440; 4061718.42 north; 594750.02 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T04:51:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868701.44 2024-11-06T04:51:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:43Z- waiting for 1730868705.44, time is 1730868703.07 2024-11-06T04:51:43Z- AHRS_M2 msg: 1730868703.45 epoch sec; seqNo:2284 2024-11-06T04:51:43Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.50 psi 2024-11-06T04:51:43Z- handleDepth()- Depth_Keller msg: 1730868703.45 epoch sec; seqNo:2441; depth 50.13 2024-11-06T04:51:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868703.46 epoch sec; seqNo:2441; 4061718.44 north; 594751.24 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T04:51:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868701.44 2024-11-06T04:51:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:43Z- waiting for 1730868705.44, time is 1730868703.46 2024-11-06T04:51:43Z- AHRS_M2 msg: 1730868703.85 epoch sec; seqNo:2285 2024-11-06T04:51:43Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.50 psi 2024-11-06T04:51:43Z- handleDepth()- Depth_Keller msg: 1730868703.86 epoch sec; seqNo:2442; depth 50.13 2024-11-06T04:51:43Z- handleDVL() - DVL msg epochMillisec: 1730868703.88 2024-11-06T04:51:43Z- handleDvl() - RDI_Pathfinder msg: 1730868703.88 sec; seqNo:899; #beams:4 2024-11-06T04:51:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.23 , 33.30 , 34.32 , 38.06 2024-11-06T04:51:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.05 , -0.01 2024-11-06T04:51:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868703.91 epoch sec; seqNo:2442; 4061718.44 north; 594751.24 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T04:51:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868701.44 2024-11-06T04:51:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:44Z- waiting for 1730868705.44, time is 1730868703.91 2024-11-06T04:51:44Z- AHRS_M2 msg: 1730868704.26 epoch sec; seqNo:2286 2024-11-06T04:51:44Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.50 psi 2024-11-06T04:51:44Z- handleDepth()- Depth_Keller msg: 1730868704.26 epoch sec; seqNo:2443; depth 50.14 2024-11-06T04:51:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868704.27 epoch sec; seqNo:2443; 4061718.44 north; 594751.24 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:51:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868701.44 2024-11-06T04:51:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:44Z- waiting for 1730868705.44, time is 1730868704.27 2024-11-06T04:51:44Z- AHRS_M2 msg: 1730868704.66 epoch sec; seqNo:2287 2024-11-06T04:51:44Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.50 psi 2024-11-06T04:51:44Z- handleDepth()- Depth_Keller msg: 1730868704.67 epoch sec; seqNo:2444; depth 50.16 2024-11-06T04:51:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868704.67 epoch sec; seqNo:2444; 4061718.44 north; 594751.24 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T04:51:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868701.44 2024-11-06T04:51:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:44Z- waiting for 1730868705.44, time is 1730868704.67 2024-11-06T04:51:45Z- AHRS_M2 msg: 1730868705.07 epoch sec; seqNo:2288 2024-11-06T04:51:45Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.50 psi 2024-11-06T04:51:45Z- handleDepth()- Depth_Keller msg: 1730868705.08 epoch sec; seqNo:2445; depth 50.16 2024-11-06T04:51:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868705.09 epoch sec; seqNo:2445; 4061718.45 north; 594752.46 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T04:51:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868701.44 2024-11-06T04:51:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:45Z- waiting for 1730868705.44, time is 1730868705.09 2024-11-06T04:51:45Z- AHRS_M2 msg: 1730868705.47 epoch sec; seqNo:2289 2024-11-06T04:51:45Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.50 psi 2024-11-06T04:51:45Z- handleDepth()- Depth_Keller msg: 1730868705.48 epoch sec; seqNo:2446; depth 50.17 2024-11-06T04:51:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868705.49 epoch sec; seqNo:2446; 4061718.45 north; 594752.46 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T04:51:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868701.44 2024-11-06T04:51:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:45Z- waiting for 1730868705.44, time is 1730868705.49 2024-11-06T04:51:45Z- LcmTrn::updateTrn() >>>> pose time 1730868705.49, meas time 1730868703.88 <<<< 2024-11-06T04:51:45Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868705.49 2024-11-06T04:51:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868703.88 sec, ping # 0; waiting for INS data more recent than 1730868701.44... 2024-11-06T04:51:45Z- TerrainNav::motionUpdate() - currEstimate = (4061718, 594752.46, 50.17) 2024-11-06T04:51:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:51:45Z- TerrainNav:: SubcloudNIS is 0.47, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:51:45Z- TNavPF::Projecting Measurements 2024-11-06T04:51:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:51:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:51:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:51:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:51:45Z- TNavFilter::4 good beams. 2024-11-06T04:51:45Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:51:45Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:51:45Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:51:45Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:51:45Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:51:45Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:51:45Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:51:45Z- TerrainMapOctree: (vehN, vehE ) = (4061718.28, 594751.01). 2024-11-06T04:51:45Z- TerrainMapOctree: Min Distance = 31.73. 2024-11-06T04:51:45Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:51:45Z- Forcing Subcloud Comparison 2024-11-06T04:51:45Z- Weighting particles with subcloud comparison 2024-11-06T04:51:45Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:45Z- meanExpectedMeasDiff: -4.825142 delta_rms_squared: 1.919035 alpha: 0.989686 2024-11-06T04:51:45Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:45Z- meanExpectedMeasDiff: 0.667860 delta_rms_squared: 1.325395 alpha: 0.985134 2024-11-06T04:51:45Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:45Z- meanExpectedMeasDiff: -1.396900 delta_rms_squared: 1.221672 alpha: 0.983893 2024-11-06T04:51:45Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:45Z- meanExpectedMeasDiff: 1.621668 delta_rms_squared: 1.654888 alpha: 0.988059 2024-11-06T04:51:45Z- Subcloud NIS: 0.476558 2024-11-06T04:51:45Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:51:45Z- TNavPF:: sumWeights = 0.026409 2024-11-06T04:51:45Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:51:45Z- TNavPF::First Term of Map Covariance Matrix is 0.45 2024-11-06T04:51:45Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T04:51:45Z- TerrainNav::Resampling particle filter... 2024-11-06T04:51:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868703.88 sec. 2024-11-06T04:51:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:51:45Z- variances([0], [2], and psi_berg) = 0.549305 0.382131 0.000000 2024-11-06T04:51:45Z- publishEstimatesDecoder() - at 1730868705.49 sec 2024-11-06T04:51:45Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:51:45Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061662.63 594756.18 52.38 2024-11-06T04:51:45Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061662.35 594757.26 53.83 lat/lon: 36.70 -121.94 2024-11-06T04:51:45Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.55 0.38 0.71 0.00 2024-11-06T04:51:45Z- handleDepth()- Depth_Keller msg: 1730868705.93 epoch sec; seqNo:2447; depth 50.17 2024-11-06T04:51:45Z- handleDVL() - DVL msg epochMillisec: 1730868705.95 2024-11-06T04:51:45Z- handleDvl() - RDI_Pathfinder msg: 1730868705.95 sec; seqNo:900; #beams:4 2024-11-06T04:51:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.21 , 32.96 , 35.68 , 38.06 2024-11-06T04:51:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , -0.01 2024-11-06T04:51:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868705.97 epoch sec; seqNo:2447; 4061718.45 north; 594752.46 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T04:51:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868705.49 2024-11-06T04:51:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:46Z- waiting for 1730868709.49, time is 1730868705.97 2024-11-06T04:51:46Z- handleDepth()- Depth_Keller msg: 1730868706.31 epoch sec; seqNo:2448; depth 50.19 2024-11-06T04:51:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868706.32 epoch sec; seqNo:2448; 4061718.84 north; 594753.68 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:51:46Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:51:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868705.49 2024-11-06T04:51:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:46Z- waiting for 1730868709.49, time is 1730868706.32 2024-11-06T04:51:46Z- AHRS_M2 msg: 1730868706.68 epoch sec; seqNo:2290 2024-11-06T04:51:46Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.50 psi 2024-11-06T04:51:46Z- handleDepth()- Depth_Keller msg: 1730868706.69 epoch sec; seqNo:2449; depth 50.20 2024-11-06T04:51:46Z- handleDVL() - DVL msg epochMillisec: 1730868706.69 2024-11-06T04:51:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:51:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:51:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:51:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:51:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:51:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:51:46Z- handleDvl() - RDI_Pathfinder msg: 1730868706.69 sec; seqNo:901; #beams:0 2024-11-06T04:51:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:51:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.05 , -0.01 2024-11-06T04:51:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868706.70 epoch sec; seqNo:2449; 4061718.84 north; 594753.68 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:51:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868705.49 2024-11-06T04:51:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:46Z- waiting for 1730868709.49, time is 1730868706.70 2024-11-06T04:51:47Z- AHRS_M2 msg: 1730868707.09 epoch sec; seqNo:2291 2024-11-06T04:51:47Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.50 psi 2024-11-06T04:51:47Z- handleDepth()- Depth_Keller msg: 1730868707.09 epoch sec; seqNo:2450; depth 50.22 2024-11-06T04:51:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868707.11 epoch sec; seqNo:2450; 4061718.84 north; 594753.68 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T04:51:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868705.49 2024-11-06T04:51:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:47Z- waiting for 1730868709.49, time is 1730868707.11 2024-11-06T04:51:47Z- AHRS_M2 msg: 1730868707.49 epoch sec; seqNo:2292 2024-11-06T04:51:47Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.50 psi 2024-11-06T04:51:47Z- handleDepth()- Depth_Keller msg: 1730868707.49 epoch sec; seqNo:2451; depth 50.22 2024-11-06T04:51:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868707.50 epoch sec; seqNo:2451; 4061718.84 north; 594753.68 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T04:51:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868705.49 2024-11-06T04:51:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:47Z- waiting for 1730868709.49, time is 1730868707.50 2024-11-06T04:51:47Z- AHRS_M2 msg: 1730868707.89 epoch sec; seqNo:2293 2024-11-06T04:51:47Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.50 psi 2024-11-06T04:51:47Z- handleDepth()- Depth_Keller msg: 1730868707.90 epoch sec; seqNo:2452; depth 50.22 2024-11-06T04:51:47Z- handleDVL() - DVL msg epochMillisec: 1730868707.92 2024-11-06T04:51:47Z- handleDvl() - RDI_Pathfinder msg: 1730868707.92 sec; seqNo:902; #beams:4 2024-11-06T04:51:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.55 , 32.79 , 35.51 , 38.06 2024-11-06T04:51:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , 0.00 2024-11-06T04:51:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868707.95 epoch sec; seqNo:2452; 4061718.86 north; 594754.90 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T04:51:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868705.49 2024-11-06T04:51:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:48Z- waiting for 1730868709.49, time is 1730868707.95 2024-11-06T04:51:48Z- AHRS_M2 msg: 1730868708.30 epoch sec; seqNo:2294 2024-11-06T04:51:48Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.50 psi 2024-11-06T04:51:48Z- handleDepth()- Depth_Keller msg: 1730868708.31 epoch sec; seqNo:2453; depth 50.22 2024-11-06T04:51:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868708.33 epoch sec; seqNo:2453; 4061718.86 north; 594754.90 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T04:51:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868705.49 2024-11-06T04:51:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:48Z- waiting for 1730868709.49, time is 1730868708.33 2024-11-06T04:51:48Z- AHRS_M2 msg: 1730868708.70 epoch sec; seqNo:2295 2024-11-06T04:51:48Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.50 psi 2024-11-06T04:51:48Z- handleDepth()- Depth_Keller msg: 1730868708.70 epoch sec; seqNo:2454; depth 50.24 2024-11-06T04:51:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868708.71 epoch sec; seqNo:2454; 4061718.86 north; 594754.90 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:51:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868705.49 2024-11-06T04:51:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:48Z- waiting for 1730868709.49, time is 1730868708.71 2024-11-06T04:51:49Z- AHRS_M2 msg: 1730868709.10 epoch sec; seqNo:2296 2024-11-06T04:51:49Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.50 psi 2024-11-06T04:51:49Z- handleDepth()- Depth_Keller msg: 1730868709.11 epoch sec; seqNo:2455; depth 50.24 2024-11-06T04:51:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868709.12 epoch sec; seqNo:2455; 4061718.87 north; 594756.12 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:51:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868705.49 2024-11-06T04:51:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:49Z- waiting for 1730868709.49, time is 1730868709.12 2024-11-06T04:51:49Z- AHRS_M2 msg: 1730868709.51 epoch sec; seqNo:2297 2024-11-06T04:51:49Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.50 psi 2024-11-06T04:51:49Z- handleDepth()- Depth_Keller msg: 1730868709.51 epoch sec; seqNo:2456; depth 50.24 2024-11-06T04:51:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868709.53 epoch sec; seqNo:2456; 4061718.87 north; 594756.12 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:51:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868705.49 2024-11-06T04:51:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:49Z- waiting for 1730868709.49, time is 1730868709.53 2024-11-06T04:51:49Z- LcmTrn::updateTrn() >>>> pose time 1730868709.53, meas time 1730868707.92 <<<< 2024-11-06T04:51:49Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868709.53 2024-11-06T04:51:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868707.92 sec, ping # 0; waiting for INS data more recent than 1730868705.49... 2024-11-06T04:51:49Z- TerrainNav::motionUpdate() - currEstimate = (4061719, 594756.12, 50.24) 2024-11-06T04:51:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:51:49Z- TerrainNav:: SubcloudNIS is 0.48, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:51:49Z- TNavPF::Projecting Measurements 2024-11-06T04:51:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:51:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:51:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:51:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:51:49Z- TNavFilter::4 good beams. 2024-11-06T04:51:49Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:51:49Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:51:49Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:51:49Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:51:49Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:51:49Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:51:49Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:51:49Z- TerrainMapOctree: (vehN, vehE ) = (4061718.70, 594754.66). 2024-11-06T04:51:49Z- TerrainMapOctree: Min Distance = 31.64. 2024-11-06T04:51:49Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:51:49Z- Forcing Subcloud Comparison 2024-11-06T04:51:49Z- Weighting particles with subcloud comparison 2024-11-06T04:51:49Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:49Z- meanExpectedMeasDiff: -4.799784 delta_rms_squared: 1.753130 alpha: 0.988721 2024-11-06T04:51:49Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:49Z- meanExpectedMeasDiff: 0.896205 delta_rms_squared: 1.095580 alpha: 0.982072 2024-11-06T04:51:49Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:49Z- meanExpectedMeasDiff: 1.185531 delta_rms_squared: 1.220608 alpha: 0.983879 2024-11-06T04:51:49Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:49Z- meanExpectedMeasDiff: 2.497586 delta_rms_squared: 1.364299 alpha: 0.985552 2024-11-06T04:51:49Z- Subcloud NIS: 0.498626 2024-11-06T04:51:49Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:51:49Z- TNavPF:: sumWeights = 0.012977 2024-11-06T04:51:49Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:51:49Z- TNavPF::First Term of Map Covariance Matrix is 0.28 2024-11-06T04:51:49Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:51:49Z- TerrainNav::Resampling particle filter... 2024-11-06T04:51:49Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868707.92 sec. 2024-11-06T04:51:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:51:49Z- variances([0], [2], and psi_berg) = 0.650223 0.440346 0.000000 2024-11-06T04:51:49Z- publishEstimatesDecoder() - at 1730868709.53 sec 2024-11-06T04:51:49Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:51:49Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061663.10 594760.02 54.83 2024-11-06T04:51:49Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061662.68 594760.97 54.00 lat/lon: 36.70 -121.94 2024-11-06T04:51:49Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.65 0.44 0.69 0.00 2024-11-06T04:51:49Z- AHRS_M2 msg: 1730868709.91 epoch sec; seqNo:2298 2024-11-06T04:51:49Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.51 psi 2024-11-06T04:51:49Z- handleDepth()- Depth_Keller msg: 1730868709.92 epoch sec; seqNo:2457; depth 50.24 2024-11-06T04:51:49Z- handleDVL() - DVL msg epochMillisec: 1730868709.95 2024-11-06T04:51:49Z- handleDvl() - RDI_Pathfinder msg: 1730868709.95 sec; seqNo:903; #beams:4 2024-11-06T04:51:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 0.00 , 32.79 , 36.02 , 37.72 2024-11-06T04:51:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , -0.01 2024-11-06T04:51:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868709.96 epoch sec; seqNo:2457; 4061718.87 north; 594756.12 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:51:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868709.53 2024-11-06T04:51:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:50Z- waiting for 1730868713.53, time is 1730868709.96 2024-11-06T04:51:50Z- AHRS_M2 msg: 1730868710.32 epoch sec; seqNo:2299 2024-11-06T04:51:50Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.51 psi 2024-11-06T04:51:50Z- handleDepth()- Depth_Keller msg: 1730868710.32 epoch sec; seqNo:2458; depth 50.24 2024-11-06T04:51:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868710.34 epoch sec; seqNo:2458; 4061718.88 north; 594757.34 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:51:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868709.53 2024-11-06T04:51:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:50Z- waiting for 1730868713.53, time is 1730868710.34 2024-11-06T04:51:50Z- AHRS_M2 msg: 1730868710.72 epoch sec; seqNo:2300 2024-11-06T04:51:50Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.51 psi 2024-11-06T04:51:50Z- handleDepth()- Depth_Keller msg: 1730868710.73 epoch sec; seqNo:2459; depth 50.23 2024-11-06T04:51:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868710.74 epoch sec; seqNo:2459; 4061718.88 north; 594757.34 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T04:51:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868709.53 2024-11-06T04:51:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:50Z- waiting for 1730868713.53, time is 1730868710.74 2024-11-06T04:51:51Z- AHRS_M2 msg: 1730868711.12 epoch sec; seqNo:2301 2024-11-06T04:51:51Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.51 psi 2024-11-06T04:51:51Z- handleDepth()- Depth_Keller msg: 1730868711.13 epoch sec; seqNo:2460; depth 50.26 2024-11-06T04:51:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868711.15 epoch sec; seqNo:2460; 4061718.88 north; 594757.34 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:51:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868709.53 2024-11-06T04:51:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:51Z- waiting for 1730868713.53, time is 1730868711.15 2024-11-06T04:51:51Z- AHRS_M2 msg: 1730868711.53 epoch sec; seqNo:2302 2024-11-06T04:51:51Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.51 psi 2024-11-06T04:51:51Z- handleDepth()- Depth_Keller msg: 1730868711.54 epoch sec; seqNo:2461; depth 50.27 2024-11-06T04:51:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868711.55 epoch sec; seqNo:2461; 4061719.26 north; 594757.34 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T04:51:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868709.53 2024-11-06T04:51:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:51Z- waiting for 1730868713.53, time is 1730868711.55 2024-11-06T04:51:51Z- AHRS_M2 msg: 1730868711.93 epoch sec; seqNo:2303 2024-11-06T04:51:51Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.51 psi 2024-11-06T04:51:51Z- handleDepth()- Depth_Keller msg: 1730868711.95 epoch sec; seqNo:2462; depth 50.25 2024-11-06T04:51:51Z- handleDVL() - DVL msg epochMillisec: 1730868711.97 2024-11-06T04:51:51Z- handleDvl() - RDI_Pathfinder msg: 1730868711.97 sec; seqNo:904; #beams:4 2024-11-06T04:51:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.11 , 32.11 , 35.85 , 37.54 2024-11-06T04:51:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , -0.03 2024-11-06T04:51:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868711.98 epoch sec; seqNo:2462; 4061719.28 north; 594758.56 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T04:51:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868709.53 2024-11-06T04:51:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:52Z- waiting for 1730868713.53, time is 1730868711.98 2024-11-06T04:51:52Z- AHRS_M2 msg: 1730868712.34 epoch sec; seqNo:2304 2024-11-06T04:51:52Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 1.51 psi 2024-11-06T04:51:52Z- handleDepth()- Depth_Keller msg: 1730868712.34 epoch sec; seqNo:2463; depth 50.26 2024-11-06T04:51:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868712.36 epoch sec; seqNo:2463; 4061719.28 north; 594758.56 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:51:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868709.53 2024-11-06T04:51:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:52Z- waiting for 1730868713.53, time is 1730868712.36 2024-11-06T04:51:52Z- AHRS_M2 msg: 1730868712.74 epoch sec; seqNo:2305 2024-11-06T04:51:52Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.51 psi 2024-11-06T04:51:52Z- handleDepth()- Depth_Keller msg: 1730868712.75 epoch sec; seqNo:2464; depth 50.28 2024-11-06T04:51:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868712.76 epoch sec; seqNo:2464; 4061719.28 north; 594758.56 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T04:51:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868709.53 2024-11-06T04:51:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:52Z- waiting for 1730868713.53, time is 1730868712.76 2024-11-06T04:51:53Z- AHRS_M2 msg: 1730868713.14 epoch sec; seqNo:2306 2024-11-06T04:51:53Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.51 psi 2024-11-06T04:51:53Z- handleDepth()- Depth_Keller msg: 1730868713.15 epoch sec; seqNo:2465; depth 50.27 2024-11-06T04:51:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868713.16 epoch sec; seqNo:2465; 4061719.29 north; 594759.78 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T04:51:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868709.53 2024-11-06T04:51:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:53Z- waiting for 1730868713.53, time is 1730868713.16 2024-11-06T04:51:53Z- AHRS_M2 msg: 1730868713.55 epoch sec; seqNo:2307 2024-11-06T04:51:53Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.51 psi 2024-11-06T04:51:53Z- handleDepth()- Depth_Keller msg: 1730868713.55 epoch sec; seqNo:2466; depth 50.29 2024-11-06T04:51:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868713.56 epoch sec; seqNo:2466; 4061719.29 north; 594759.78 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:51:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868709.53 2024-11-06T04:51:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:53Z- waiting for 1730868713.53, time is 1730868713.56 2024-11-06T04:51:53Z- LcmTrn::updateTrn() >>>> pose time 1730868713.56, meas time 1730868711.97 <<<< 2024-11-06T04:51:53Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868713.56 2024-11-06T04:51:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868711.97 sec, ping # 0; waiting for INS data more recent than 1730868709.53... 2024-11-06T04:51:53Z- TerrainNav::motionUpdate() - currEstimate = (4061719, 594759.78, 50.29) 2024-11-06T04:51:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:51:53Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:51:53Z- TNavPF::Projecting Measurements 2024-11-06T04:51:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:51:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:51:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:51:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:51:53Z- TNavFilter::4 good beams. 2024-11-06T04:51:53Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:51:53Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:51:53Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:51:53Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:51:53Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:51:53Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:51:53Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:51:53Z- TerrainMapOctree: (vehN, vehE ) = (4061719.12, 594758.33). 2024-11-06T04:51:53Z- TerrainMapOctree: Min Distance = 31.98. 2024-11-06T04:51:53Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:51:53Z- Forcing Subcloud Comparison 2024-11-06T04:51:53Z- Weighting particles with subcloud comparison 2024-11-06T04:51:53Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:53Z- meanExpectedMeasDiff: 0.116020 delta_rms_squared: 1.555439 alpha: 0.987305 2024-11-06T04:51:53Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:53Z- meanExpectedMeasDiff: 0.432118 delta_rms_squared: 1.054629 alpha: 0.981389 2024-11-06T04:51:53Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:53Z- meanExpectedMeasDiff: 1.607560 delta_rms_squared: 1.240385 alpha: 0.984132 2024-11-06T04:51:53Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:53Z- meanExpectedMeasDiff: 2.293402 delta_rms_squared: 1.309375 alpha: 0.984955 2024-11-06T04:51:53Z- Subcloud NIS: 0.502293 2024-11-06T04:51:53Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:51:53Z- TNavPF:: sumWeights = 0.339234 2024-11-06T04:51:53Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:51:53Z- TNavPF::First Term of Map Covariance Matrix is 0.28 2024-11-06T04:51:53Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T04:51:53Z- TerrainNav::Resampling particle filter... 2024-11-06T04:51:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868711.97 sec. 2024-11-06T04:51:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:51:53Z- variances([0], [2], and psi_berg) = 0.643281 0.448475 0.000000 2024-11-06T04:51:53Z- publishEstimatesDecoder() - at 1730868713.56 sec 2024-11-06T04:51:53Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:51:53Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061663.31 594763.52 53.10 2024-11-06T04:51:53Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061663.13 594764.62 53.51 lat/lon: 36.70 -121.94 2024-11-06T04:51:53Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.64 0.45 0.66 0.00 2024-11-06T04:51:53Z- AHRS_M2 msg: 1730868713.95 epoch sec; seqNo:2308 2024-11-06T04:51:53Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.51 psi 2024-11-06T04:51:53Z- handleDepth()- Depth_Keller msg: 1730868713.96 epoch sec; seqNo:2467; depth 50.30 2024-11-06T04:51:53Z- handleDVL() - DVL msg epochMillisec: 1730868713.98 2024-11-06T04:51:53Z- handleDvl() - RDI_Pathfinder msg: 1730868713.98 sec; seqNo:905; #beams:4 2024-11-06T04:51:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.11 , 31.43 , 35.68 , 37.38 2024-11-06T04:51:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.06 , -0.02 2024-11-06T04:51:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868713.99 epoch sec; seqNo:2467; 4061719.29 north; 594759.78 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:51:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868713.56 2024-11-06T04:51:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:54Z- waiting for 1730868717.56, time is 1730868713.99 2024-11-06T04:51:54Z- AHRS_M2 msg: 1730868714.36 epoch sec; seqNo:2309 2024-11-06T04:51:54Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.51 psi 2024-11-06T04:51:54Z- handleDepth()- Depth_Keller msg: 1730868714.36 epoch sec; seqNo:2468; depth 50.31 2024-11-06T04:51:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868714.38 epoch sec; seqNo:2468; 4061719.29 north; 594759.78 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:51:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868713.56 2024-11-06T04:51:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:54Z- waiting for 1730868717.56, time is 1730868714.38 2024-11-06T04:51:54Z- AHRS_M2 msg: 1730868714.76 epoch sec; seqNo:2310 2024-11-06T04:51:54Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.51 psi 2024-11-06T04:51:54Z- handleDepth()- Depth_Keller msg: 1730868714.77 epoch sec; seqNo:2469; depth 50.31 2024-11-06T04:51:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868714.79 epoch sec; seqNo:2469; 4061719.30 north; 594761.00 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:51:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868713.56 2024-11-06T04:51:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:54Z- waiting for 1730868717.56, time is 1730868714.79 2024-11-06T04:51:55Z- AHRS_M2 msg: 1730868715.16 epoch sec; seqNo:2311 2024-11-06T04:51:55Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.52 psi 2024-11-06T04:51:55Z- handleDepth()- Depth_Keller msg: 1730868715.17 epoch sec; seqNo:2470; depth 50.33 2024-11-06T04:51:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868715.18 epoch sec; seqNo:2470; 4061719.30 north; 594761.00 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T04:51:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868713.56 2024-11-06T04:51:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:55Z- waiting for 1730868717.56, time is 1730868715.18 2024-11-06T04:51:55Z- AHRS_M2 msg: 1730868715.57 epoch sec; seqNo:2312 2024-11-06T04:51:55Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.52 psi 2024-11-06T04:51:55Z- handleDepth()- Depth_Keller msg: 1730868715.58 epoch sec; seqNo:2471; depth 50.32 2024-11-06T04:51:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868715.59 epoch sec; seqNo:2471; 4061719.30 north; 594761.00 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:51:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868713.56 2024-11-06T04:51:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:55Z- waiting for 1730868717.56, time is 1730868715.59 2024-11-06T04:51:55Z- AHRS_M2 msg: 1730868715.97 epoch sec; seqNo:2313 2024-11-06T04:51:55Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.52 psi 2024-11-06T04:51:55Z- handleDepth()- Depth_Keller msg: 1730868715.98 epoch sec; seqNo:2472; depth 50.34 2024-11-06T04:51:56Z- handleDVL() - DVL msg epochMillisec: 1730868716.00 2024-11-06T04:51:56Z- handleDvl() - RDI_Pathfinder msg: 1730868716.00 sec; seqNo:906; #beams:4 2024-11-06T04:51:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.95 , 30.92 , 35.51 , 37.21 2024-11-06T04:51:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.06 , 0.00 2024-11-06T04:51:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868716.02 epoch sec; seqNo:2472; 4061719.68 north; 594760.99 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:51:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868713.56 2024-11-06T04:51:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:56Z- waiting for 1730868717.56, time is 1730868716.02 2024-11-06T04:51:56Z- AHRS_M2 msg: 1730868716.38 epoch sec; seqNo:2314 2024-11-06T04:51:56Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.52 psi 2024-11-06T04:51:56Z- handleDepth()- Depth_Keller msg: 1730868716.39 epoch sec; seqNo:2473; depth 50.34 2024-11-06T04:51:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868716.40 epoch sec; seqNo:2473; 4061719.70 north; 594762.21 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:51:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868713.56 2024-11-06T04:51:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:56Z- waiting for 1730868717.56, time is 1730868716.40 2024-11-06T04:51:56Z- AHRS_M2 msg: 1730868716.78 epoch sec; seqNo:2315 2024-11-06T04:51:56Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.52 psi 2024-11-06T04:51:56Z- handleDepth()- Depth_Keller msg: 1730868716.79 epoch sec; seqNo:2474; depth 50.35 2024-11-06T04:51:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868716.80 epoch sec; seqNo:2474; 4061719.70 north; 594762.21 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:51:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868713.56 2024-11-06T04:51:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:56Z- waiting for 1730868717.56, time is 1730868716.80 2024-11-06T04:51:57Z- AHRS_M2 msg: 1730868717.20 epoch sec; seqNo:2316 2024-11-06T04:51:57Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.52 psi 2024-11-06T04:51:57Z- handleDepth()- Depth_Keller msg: 1730868717.20 epoch sec; seqNo:2475; depth 50.37 2024-11-06T04:51:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868717.21 epoch sec; seqNo:2475; 4061719.70 north; 594762.21 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:51:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868713.56 2024-11-06T04:51:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:57Z- waiting for 1730868717.56, time is 1730868717.21 2024-11-06T04:51:57Z- AHRS_M2 msg: 1730868717.59 epoch sec; seqNo:2317 2024-11-06T04:51:57Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.52 psi 2024-11-06T04:51:57Z- handleDepth()- Depth_Keller msg: 1730868717.62 epoch sec; seqNo:2476; depth 50.38 2024-11-06T04:51:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868717.66 epoch sec; seqNo:2476; 4061719.71 north; 594763.43 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:51:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868713.56 2024-11-06T04:51:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:57Z- waiting for 1730868717.56, time is 1730868717.66 2024-11-06T04:51:57Z- LcmTrn::updateTrn() >>>> pose time 1730868717.66, meas time 1730868716.00 <<<< 2024-11-06T04:51:57Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868717.66 2024-11-06T04:51:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868716.00 sec, ping # 0; waiting for INS data more recent than 1730868713.56... 2024-11-06T04:51:57Z- TerrainNav::motionUpdate() - currEstimate = (4061720, 594763.43, 50.38) 2024-11-06T04:51:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:51:57Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:51:57Z- TNavPF::Projecting Measurements 2024-11-06T04:51:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:51:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:51:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:51:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:51:57Z- TNavFilter::4 good beams. 2024-11-06T04:51:57Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:51:57Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:51:57Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:51:57Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:51:57Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:51:57Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:51:57Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:51:57Z- TerrainMapOctree: (vehN, vehE ) = (4061719.54, 594761.96). 2024-11-06T04:51:57Z- TerrainMapOctree: Min Distance = 32.72. 2024-11-06T04:51:57Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:51:57Z- Forcing Subcloud Comparison 2024-11-06T04:51:57Z- Weighting particles with subcloud comparison 2024-11-06T04:51:57Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:57Z- meanExpectedMeasDiff: -0.308307 delta_rms_squared: 1.151242 alpha: 0.982924 2024-11-06T04:51:57Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:57Z- meanExpectedMeasDiff: -0.999309 delta_rms_squared: 1.077557 alpha: 0.981778 2024-11-06T04:51:57Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:57Z- meanExpectedMeasDiff: 0.658826 delta_rms_squared: 1.141468 alpha: 0.982780 2024-11-06T04:51:57Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:51:57Z- meanExpectedMeasDiff: 1.454810 delta_rms_squared: 1.268723 alpha: 0.984481 2024-11-06T04:51:57Z- Subcloud NIS: 0.483779 2024-11-06T04:51:57Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:51:57Z- TNavPF:: sumWeights = 0.418250 2024-11-06T04:51:57Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:51:57Z- TNavPF::First Term of Map Covariance Matrix is 0.27 2024-11-06T04:51:57Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T04:51:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868716.00 sec. 2024-11-06T04:51:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:51:57Z- variances([0], [2], and psi_berg) = 0.644305 0.461499 0.000000 2024-11-06T04:51:57Z- publishEstimatesDecoder() - at 1730868717.66 sec 2024-11-06T04:51:57Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:51:57Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061663.56 594768.81 52.72 2024-11-06T04:51:57Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061663.52 594768.33 53.48 lat/lon: 36.70 -121.94 2024-11-06T04:51:57Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.64 0.46 0.70 0.00 2024-11-06T04:51:58Z- AHRS_M2 msg: 1730868717.99 epoch sec; seqNo:2318 2024-11-06T04:51:58Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.52 psi 2024-11-06T04:51:58Z- handleDepth()- Depth_Keller msg: 1730868718.02 epoch sec; seqNo:2477; depth 50.40 2024-11-06T04:51:58Z- handleDVL() - DVL msg epochMillisec: 1730868718.06 2024-11-06T04:51:58Z- handleDvl() - RDI_Pathfinder msg: 1730868718.06 sec; seqNo:907; #beams:4 2024-11-06T04:51:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.78 , 30.75 , 35.85 , 37.38 2024-11-06T04:51:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.05 , 0.02 2024-11-06T04:51:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868718.07 epoch sec; seqNo:2477; 4061719.71 north; 594763.43 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T04:51:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868717.66 2024-11-06T04:51:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:58Z- waiting for 1730868721.66, time is 1730868718.07 2024-11-06T04:51:58Z- AHRS_M2 msg: 1730868718.40 epoch sec; seqNo:2319 2024-11-06T04:51:58Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.52 psi 2024-11-06T04:51:58Z- handleDepth()- Depth_Keller msg: 1730868718.40 epoch sec; seqNo:2478; depth 50.41 2024-11-06T04:51:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868718.41 epoch sec; seqNo:2478; 4061719.71 north; 594763.43 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T04:51:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868717.66 2024-11-06T04:51:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:58Z- waiting for 1730868721.66, time is 1730868718.41 2024-11-06T04:51:58Z- handleDepth()- Depth_Keller msg: 1730868718.82 epoch sec; seqNo:2479; depth 50.40 2024-11-06T04:51:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868718.83 epoch sec; seqNo:2479; 4061719.71 north; 594763.43 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T04:51:58Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:51:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868717.66 2024-11-06T04:51:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:58Z- waiting for 1730868721.66, time is 1730868718.83 2024-11-06T04:51:59Z- AHRS_M2 msg: 1730868719.21 epoch sec; seqNo:2320 2024-11-06T04:51:59Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.52 psi 2024-11-06T04:51:59Z- handleDepth()- Depth_Keller msg: 1730868719.21 epoch sec; seqNo:2480; depth 50.41 2024-11-06T04:51:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868719.24 epoch sec; seqNo:2480; 4061719.72 north; 594764.65 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T04:51:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:51:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868717.66 2024-11-06T04:51:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:59Z- waiting for 1730868721.66, time is 1730868719.24 2024-11-06T04:51:59Z- AHRS_M2 msg: 1730868719.64 epoch sec; seqNo:2321 2024-11-06T04:51:59Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 1.52 psi 2024-11-06T04:51:59Z- handleDepth()- Depth_Keller msg: 1730868719.66 epoch sec; seqNo:2481; depth 50.42 2024-11-06T04:51:59Z- handleDVL() - DVL msg epochMillisec: 1730868719.68 2024-11-06T04:51:59Z- handleDvl() - RDI_Pathfinder msg: 1730868719.68 sec; seqNo:908; #beams:4 2024-11-06T04:51:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.77 , 30.92 , 35.85 , 37.21 2024-11-06T04:51:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.03 2024-11-06T04:51:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868719.73 epoch sec; seqNo:2481; 4061720.10 north; 594764.65 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T04:51:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:51:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868717.66 2024-11-06T04:51:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:51:59Z- waiting for 1730868721.66, time is 1730868719.73 2024-11-06T04:52:00Z- AHRS_M2 msg: 1730868720.01 epoch sec; seqNo:2322 2024-11-06T04:52:00Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.52 psi 2024-11-06T04:52:00Z- handleDepth()- Depth_Keller msg: 1730868720.02 epoch sec; seqNo:2482; depth 50.43 2024-11-06T04:52:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868720.03 epoch sec; seqNo:2482; 4061720.10 north; 594764.65 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:52:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868717.66 2024-11-06T04:52:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:00Z- waiting for 1730868721.66, time is 1730868720.03 2024-11-06T04:52:00Z- AHRS_M2 msg: 1730868720.41 epoch sec; seqNo:2323 2024-11-06T04:52:00Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 1.52 psi 2024-11-06T04:52:00Z- handleDepth()- Depth_Keller msg: 1730868720.43 epoch sec; seqNo:2483; depth 50.43 2024-11-06T04:52:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868720.44 epoch sec; seqNo:2483; 4061720.11 north; 594765.87 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:52:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868717.66 2024-11-06T04:52:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:00Z- waiting for 1730868721.66, time is 1730868720.44 2024-11-06T04:52:00Z- AHRS_M2 msg: 1730868720.83 epoch sec; seqNo:2324 2024-11-06T04:52:00Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 1.52 psi 2024-11-06T04:52:00Z- handleDepth()- Depth_Keller msg: 1730868720.83 epoch sec; seqNo:2484; depth 50.43 2024-11-06T04:52:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868720.87 epoch sec; seqNo:2484; 4061720.11 north; 594765.87 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:52:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868717.66 2024-11-06T04:52:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:00Z- waiting for 1730868721.66, time is 1730868720.87 2024-11-06T04:52:01Z- AHRS_M2 msg: 1730868721.22 epoch sec; seqNo:2325 2024-11-06T04:52:01Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 1.52 psi 2024-11-06T04:52:01Z- handleDepth()- Depth_Keller msg: 1730868721.25 epoch sec; seqNo:2485; depth 50.43 2024-11-06T04:52:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868721.26 epoch sec; seqNo:2485; 4061720.11 north; 594765.87 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:52:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868717.66 2024-11-06T04:52:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:01Z- waiting for 1730868721.66, time is 1730868721.26 2024-11-06T04:52:01Z- AHRS_M2 msg: 1730868721.67 epoch sec; seqNo:2326 2024-11-06T04:52:01Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 1.53 psi 2024-11-06T04:52:01Z- handleDepth()- Depth_Keller msg: 1730868721.67 epoch sec; seqNo:2486; depth 50.44 2024-11-06T04:52:01Z- handleDVL() - DVL msg epochMillisec: 1730868721.69 2024-11-06T04:52:01Z- handleDvl() - RDI_Pathfinder msg: 1730868721.69 sec; seqNo:909; #beams:4 2024-11-06T04:52:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.60 , 30.58 , 36.36 , 37.04 2024-11-06T04:52:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.00 , 0.00 2024-11-06T04:52:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868721.72 epoch sec; seqNo:2486; 4061720.13 north; 594767.09 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T04:52:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868717.66 2024-11-06T04:52:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:01Z- waiting for 1730868721.66, time is 1730868721.72 2024-11-06T04:52:01Z- LcmTrn::updateTrn() >>>> pose time 1730868721.72, meas time 1730868721.69 <<<< 2024-11-06T04:52:01Z- LcmTrn::updateTrn() equating pose 1730868721.69 and meas times 1730868721.69 2024-11-06T04:52:01Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868721.69 2024-11-06T04:52:01Z- TerrainNav::motionUpdate() - currEstimate = (4061720, 594767.09, 50.44) 2024-11-06T04:52:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:52:01Z- TerrainNav:: SubcloudNIS is 0.48, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:52:01Z- TNavPF::Projecting Measurements 2024-11-06T04:52:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:52:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:52:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:52:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:52:01Z- TNavFilter::4 good beams. 2024-11-06T04:52:01Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:52:01Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:52:01Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:52:01Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:52:01Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:52:01Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:52:01Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:52:01Z- TerrainMapOctree: (vehN, vehE ) = (4061720.13, 594767.09). 2024-11-06T04:52:01Z- TerrainMapOctree: Min Distance = 34.42. 2024-11-06T04:52:01Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:52:01Z- Forcing Subcloud Comparison 2024-11-06T04:52:01Z- Weighting particles with subcloud comparison 2024-11-06T04:52:01Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:01Z- meanExpectedMeasDiff: 1.593386 delta_rms_squared: 1.522690 alpha: 0.987036 2024-11-06T04:52:01Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:01Z- meanExpectedMeasDiff: -2.421050 delta_rms_squared: 1.119947 alpha: 0.982455 2024-11-06T04:52:01Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:01Z- meanExpectedMeasDiff: 0.211873 delta_rms_squared: 1.697986 alpha: 0.988358 2024-11-06T04:52:01Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:01Z- meanExpectedMeasDiff: 0.564815 delta_rms_squared: 1.376134 alpha: 0.985675 2024-11-06T04:52:01Z- Subcloud NIS: 0.481889 2024-11-06T04:52:01Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:52:01Z- TNavPF:: sumWeights = 0.188469 2024-11-06T04:52:01Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:52:01Z- TNavPF::First Term of Map Covariance Matrix is 0.44 2024-11-06T04:52:01Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.03 2024-11-06T04:52:01Z- TerrainNav::Resampling particle filter... 2024-11-06T04:52:01Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868721.69 sec, ping # 0. 2024-11-06T04:52:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:52:01Z- variances([0], [2], and psi_berg) = 0.696014 0.513869 0.000000 2024-11-06T04:52:01Z- publishEstimatesDecoder() - at 1730868721.69 sec 2024-11-06T04:52:01Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:52:01Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061664.18 594770.78 53.17 2024-11-06T04:52:01Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061663.87 594771.90 53.53 lat/lon: 36.70 -121.94 2024-11-06T04:52:01Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.51 0.44 0.00 2024-11-06T04:52:02Z- handleDepth()- Depth_Keller msg: 1730868722.05 epoch sec; seqNo:2487; depth 50.43 2024-11-06T04:52:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868722.08 epoch sec; seqNo:2487; 4061720.13 north; 594767.09 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:52:02Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:52:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868721.69 2024-11-06T04:52:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:02Z- waiting for 1730868725.69, time is 1730868722.08 2024-11-06T04:52:02Z- AHRS_M2 msg: 1730868722.44 epoch sec; seqNo:2327 2024-11-06T04:52:02Z- AHRS_M2 msg: 0.17 phi; -0.00 theta; 1.53 psi 2024-11-06T04:52:02Z- handleDepth()- Depth_Keller msg: 1730868722.44 epoch sec; seqNo:2488; depth 50.43 2024-11-06T04:52:02Z- handleDVL() - DVL msg epochMillisec: 1730868722.44 2024-11-06T04:52:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:02Z- handleDvl() - RDI_Pathfinder msg: 1730868722.44 sec; seqNo:910; #beams:0 2024-11-06T04:52:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.00 , 0.00 2024-11-06T04:52:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868722.46 epoch sec; seqNo:2488; 4061720.13 north; 594767.09 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:52:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868721.69 2024-11-06T04:52:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:02Z- waiting for 1730868725.69, time is 1730868722.46 2024-11-06T04:52:02Z- AHRS_M2 msg: 1730868722.85 epoch sec; seqNo:2328 2024-11-06T04:52:02Z- AHRS_M2 msg: 0.17 phi; -0.00 theta; 1.53 psi 2024-11-06T04:52:02Z- handleDepth()- Depth_Keller msg: 1730868722.86 epoch sec; seqNo:2489; depth 50.43 2024-11-06T04:52:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868722.87 epoch sec; seqNo:2489; 4061720.13 north; 594767.09 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:52:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868721.69 2024-11-06T04:52:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:03Z- waiting for 1730868725.69, time is 1730868722.87 2024-11-06T04:52:03Z- AHRS_M2 msg: 1730868723.25 epoch sec; seqNo:2329 2024-11-06T04:52:03Z- AHRS_M2 msg: 0.17 phi; 0.00 theta; 1.53 psi 2024-11-06T04:52:03Z- handleDepth()- Depth_Keller msg: 1730868723.25 epoch sec; seqNo:2490; depth 50.43 2024-11-06T04:52:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868723.26 epoch sec; seqNo:2490; 4061720.14 north; 594768.31 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:52:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868721.69 2024-11-06T04:52:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:03Z- waiting for 1730868725.69, time is 1730868723.26 2024-11-06T04:52:03Z- AHRS_M2 msg: 1730868723.65 epoch sec; seqNo:2330 2024-11-06T04:52:03Z- AHRS_M2 msg: 0.17 phi; 0.00 theta; 1.54 psi 2024-11-06T04:52:03Z- handleDepth()- Depth_Keller msg: 1730868723.65 epoch sec; seqNo:2491; depth 50.41 2024-11-06T04:52:03Z- handleDVL() - DVL msg epochMillisec: 1730868723.67 2024-11-06T04:52:03Z- handleDvl() - RDI_Pathfinder msg: 1730868723.67 sec; seqNo:911; #beams:4 2024-11-06T04:52:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.95 , 29.73 , 36.19 , 36.53 2024-11-06T04:52:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , -0.04 2024-11-06T04:52:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868723.70 epoch sec; seqNo:2491; 4061720.14 north; 594768.31 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T04:52:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868721.69 2024-11-06T04:52:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:03Z- waiting for 1730868725.69, time is 1730868723.70 2024-11-06T04:52:04Z- AHRS_M2 msg: 1730868724.05 epoch sec; seqNo:2331 2024-11-06T04:52:04Z- AHRS_M2 msg: 0.17 phi; 0.00 theta; 1.54 psi 2024-11-06T04:52:04Z- handleDepth()- Depth_Keller msg: 1730868724.06 epoch sec; seqNo:2492; depth 50.41 2024-11-06T04:52:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868724.08 epoch sec; seqNo:2492; 4061720.14 north; 594768.31 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T04:52:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868721.69 2024-11-06T04:52:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:04Z- waiting for 1730868725.69, time is 1730868724.08 2024-11-06T04:52:04Z- AHRS_M2 msg: 1730868724.45 epoch sec; seqNo:2332 2024-11-06T04:52:04Z- AHRS_M2 msg: 0.17 phi; 0.01 theta; 1.54 psi 2024-11-06T04:52:04Z- handleDepth()- Depth_Keller msg: 1730868724.47 epoch sec; seqNo:2493; depth 50.39 2024-11-06T04:52:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868724.51 epoch sec; seqNo:2493; 4061720.16 north; 594769.53 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:52:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868721.69 2024-11-06T04:52:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:04Z- waiting for 1730868725.69, time is 1730868724.51 2024-11-06T04:52:04Z- AHRS_M2 msg: 1730868724.86 epoch sec; seqNo:2333 2024-11-06T04:52:04Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.54 psi 2024-11-06T04:52:04Z- handleDepth()- Depth_Keller msg: 1730868724.87 epoch sec; seqNo:2494; depth 50.37 2024-11-06T04:52:04Z- handleDVL() - DVL msg epochMillisec: 1730868724.90 2024-11-06T04:52:04Z- handleDvl() - RDI_Pathfinder msg: 1730868724.90 sec; seqNo:912; #beams:4 2024-11-06T04:52:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.78 , 29.56 , 36.19 , 36.53 2024-11-06T04:52:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.01 , -0.05 2024-11-06T04:52:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868724.91 epoch sec; seqNo:2494; 4061720.16 north; 594769.53 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:52:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868721.69 2024-11-06T04:52:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:05Z- waiting for 1730868725.69, time is 1730868724.91 2024-11-06T04:52:05Z- AHRS_M2 msg: 1730868725.27 epoch sec; seqNo:2334 2024-11-06T04:52:05Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.54 psi 2024-11-06T04:52:05Z- handleDepth()- Depth_Keller msg: 1730868725.27 epoch sec; seqNo:2495; depth 50.36 2024-11-06T04:52:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868725.30 epoch sec; seqNo:2495; 4061720.16 north; 594769.53 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:52:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868721.69 2024-11-06T04:52:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:05Z- waiting for 1730868725.69, time is 1730868725.30 2024-11-06T04:52:05Z- AHRS_M2 msg: 1730868725.67 epoch sec; seqNo:2335 2024-11-06T04:52:05Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.54 psi 2024-11-06T04:52:05Z- handleDepth()- Depth_Keller msg: 1730868725.67 epoch sec; seqNo:2496; depth 50.37 2024-11-06T04:52:05Z- handleDVL() - DVL msg epochMillisec: 1730868725.69 2024-11-06T04:52:05Z- handleDvl() - RDI_Pathfinder msg: 1730868725.69 sec; seqNo:913; #beams:4 2024-11-06T04:52:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.44 , 29.22 , 36.53 , 36.36 2024-11-06T04:52:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.02 , -0.05 2024-11-06T04:52:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868725.72 epoch sec; seqNo:2496; 4061720.17 north; 594770.75 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:52:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868721.69 2024-11-06T04:52:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:05Z- waiting for 1730868725.69, time is 1730868725.72 2024-11-06T04:52:05Z- LcmTrn::updateTrn() >>>> pose time 1730868725.72, meas time 1730868725.69 <<<< 2024-11-06T04:52:05Z- LcmTrn::updateTrn() equating pose 1730868725.69 and meas times 1730868725.69 2024-11-06T04:52:05Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868725.69 2024-11-06T04:52:05Z- TerrainNav::motionUpdate() - currEstimate = (4061720, 594770.75, 50.37) 2024-11-06T04:52:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:52:05Z- TerrainNav:: SubcloudNIS is 0.48, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:52:05Z- TNavPF::Projecting Measurements 2024-11-06T04:52:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:52:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:52:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:52:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:52:05Z- TNavFilter::4 good beams. 2024-11-06T04:52:05Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:52:05Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:52:05Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:52:05Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:52:05Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:52:05Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:52:05Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:52:05Z- TerrainMapOctree: (vehN, vehE ) = (4061720.17, 594770.75). 2024-11-06T04:52:05Z- TerrainMapOctree: Min Distance = 36.34. 2024-11-06T04:52:05Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:52:05Z- Forcing Subcloud Comparison 2024-11-06T04:52:05Z- Weighting particles with subcloud comparison 2024-11-06T04:52:05Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:05Z- meanExpectedMeasDiff: 2.088937 delta_rms_squared: 1.012188 alpha: 0.980624 2024-11-06T04:52:05Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:05Z- meanExpectedMeasDiff: -3.929371 delta_rms_squared: 0.817726 alpha: 0.976126 2024-11-06T04:52:05Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:05Z- meanExpectedMeasDiff: 0.773189 delta_rms_squared: 0.862982 alpha: 0.977349 2024-11-06T04:52:05Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:05Z- meanExpectedMeasDiff: -0.461909 delta_rms_squared: 1.079592 alpha: 0.981811 2024-11-06T04:52:05Z- Subcloud NIS: 0.502442 2024-11-06T04:52:05Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:52:05Z- TNavPF:: sumWeights = 0.044321 2024-11-06T04:52:05Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:52:05Z- TNavPF::First Term of Map Covariance Matrix is 0.20 2024-11-06T04:52:05Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T04:52:05Z- TerrainNav::Resampling particle filter... 2024-11-06T04:52:05Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868725.69 sec, ping # 0. 2024-11-06T04:52:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:52:05Z- variances([0], [2], and psi_berg) = 0.777705 0.546372 0.000000 2024-11-06T04:52:05Z- publishEstimatesDecoder() - at 1730868725.69 sec 2024-11-06T04:52:05Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:52:05Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061664.32 594774.50 53.88 2024-11-06T04:52:05Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061663.55 594775.91 53.65 lat/lon: 36.70 -121.94 2024-11-06T04:52:05Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.78 0.55 0.40 0.00 2024-11-06T04:52:06Z- AHRS_M2 msg: 1730868726.07 epoch sec; seqNo:2336 2024-11-06T04:52:06Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 1.54 psi 2024-11-06T04:52:06Z- handleDepth()- Depth_Keller msg: 1730868726.08 epoch sec; seqNo:2497; depth 50.35 2024-11-06T04:52:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868726.10 epoch sec; seqNo:2497; 4061720.17 north; 594770.75 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:52:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868725.69 2024-11-06T04:52:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:06Z- waiting for 1730868729.69, time is 1730868726.10 2024-11-06T04:52:06Z- handleDepth()- Depth_Keller msg: 1730868726.50 epoch sec; seqNo:2498; depth 50.35 2024-11-06T04:52:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868726.55 epoch sec; seqNo:2498; 4061720.17 north; 594770.75 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:52:06Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:52:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868725.69 2024-11-06T04:52:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:06Z- waiting for 1730868729.69, time is 1730868726.55 2024-11-06T04:52:06Z- AHRS_M2 msg: 1730868726.88 epoch sec; seqNo:2337 2024-11-06T04:52:06Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 1.54 psi 2024-11-06T04:52:06Z- handleDepth()- Depth_Keller msg: 1730868726.88 epoch sec; seqNo:2499; depth 50.34 2024-11-06T04:52:06Z- handleDVL() - DVL msg epochMillisec: 1730868726.92 2024-11-06T04:52:06Z- handleDvl() - RDI_Pathfinder msg: 1730868726.92 sec; seqNo:914; #beams:4 2024-11-06T04:52:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.93 , 28.71 , 36.53 , 36.19 2024-11-06T04:52:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , -0.05 2024-11-06T04:52:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868726.93 epoch sec; seqNo:2499; 4061720.17 north; 594770.75 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:52:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868725.69 2024-11-06T04:52:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:07Z- waiting for 1730868729.69, time is 1730868726.93 2024-11-06T04:52:07Z- AHRS_M2 msg: 1730868727.29 epoch sec; seqNo:2338 2024-11-06T04:52:07Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.54 psi 2024-11-06T04:52:07Z- handleDepth()- Depth_Keller msg: 1730868727.29 epoch sec; seqNo:2500; depth 50.34 2024-11-06T04:52:07Z- handleDVL() - DVL msg epochMillisec: 1730868727.30 2024-11-06T04:52:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:07Z- handleDvl() - RDI_Pathfinder msg: 1730868727.30 sec; seqNo:915; #beams:0 2024-11-06T04:52:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.03 , -0.05 2024-11-06T04:52:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868727.31 epoch sec; seqNo:2500; 4061720.18 north; 594771.97 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:52:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868725.69 2024-11-06T04:52:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:07Z- waiting for 1730868729.69, time is 1730868727.31 2024-11-06T04:52:07Z- handleDepth()- Depth_Keller msg: 1730868727.71 epoch sec; seqNo:2501; depth 50.32 2024-11-06T04:52:07Z- handleDVL() - DVL msg epochMillisec: 1730868727.73 2024-11-06T04:52:07Z- handleDvl() - RDI_Pathfinder msg: 1730868727.73 sec; seqNo:916; #beams:4 2024-11-06T04:52:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.59 , 29.05 , 36.36 , 36.02 2024-11-06T04:52:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , -0.03 2024-11-06T04:52:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868727.75 epoch sec; seqNo:2501; 4061720.18 north; 594771.97 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:52:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868725.69 2024-11-06T04:52:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:07Z- waiting for 1730868729.69, time is 1730868727.75 2024-11-06T04:52:08Z- AHRS_M2 msg: 1730868728.09 epoch sec; seqNo:2339 2024-11-06T04:52:08Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.53 psi 2024-11-06T04:52:08Z- handleDepth()- Depth_Keller msg: 1730868728.10 epoch sec; seqNo:2502; depth 50.32 2024-11-06T04:52:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868728.12 epoch sec; seqNo:2502; 4061720.56 north; 594771.97 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:52:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868725.69 2024-11-06T04:52:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:08Z- waiting for 1730868729.69, time is 1730868728.12 2024-11-06T04:52:08Z- AHRS_M2 msg: 1730868728.50 epoch sec; seqNo:2340 2024-11-06T04:52:08Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.53 psi 2024-11-06T04:52:08Z- handleDepth()- Depth_Keller msg: 1730868728.50 epoch sec; seqNo:2503; depth 50.31 2024-11-06T04:52:08Z- handleDVL() - DVL msg epochMillisec: 1730868728.51 2024-11-06T04:52:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:08Z- handleDvl() - RDI_Pathfinder msg: 1730868728.51 sec; seqNo:917; #beams:0 2024-11-06T04:52:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.05 , -0.03 2024-11-06T04:52:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868728.53 epoch sec; seqNo:2503; 4061720.56 north; 594771.97 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:52:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868725.69 2024-11-06T04:52:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:08Z- waiting for 1730868729.69, time is 1730868728.53 2024-11-06T04:52:08Z- AHRS_M2 msg: 1730868728.90 epoch sec; seqNo:2341 2024-11-06T04:52:08Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.53 psi 2024-11-06T04:52:08Z- handleDepth()- Depth_Keller msg: 1730868728.90 epoch sec; seqNo:2504; depth 50.32 2024-11-06T04:52:08Z- handleDVL() - DVL msg epochMillisec: 1730868728.95 2024-11-06T04:52:08Z- handleDvl() - RDI_Pathfinder msg: 1730868728.95 sec; seqNo:918; #beams:4 2024-11-06T04:52:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.24 , 30.07 , 36.19 , 36.02 2024-11-06T04:52:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , -0.01 2024-11-06T04:52:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868728.96 epoch sec; seqNo:2504; 4061720.57 north; 594773.19 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:52:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868725.69 2024-11-06T04:52:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:09Z- waiting for 1730868729.69, time is 1730868728.96 2024-11-06T04:52:09Z- AHRS_M2 msg: 1730868729.31 epoch sec; seqNo:2342 2024-11-06T04:52:09Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.53 psi 2024-11-06T04:52:09Z- handleDepth()- Depth_Keller msg: 1730868729.31 epoch sec; seqNo:2505; depth 50.30 2024-11-06T04:52:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868729.33 epoch sec; seqNo:2505; 4061720.57 north; 594773.19 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:52:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868725.69 2024-11-06T04:52:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:09Z- waiting for 1730868729.69, time is 1730868729.33 2024-11-06T04:52:09Z- AHRS_M2 msg: 1730868729.71 epoch sec; seqNo:2343 2024-11-06T04:52:09Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.53 psi 2024-11-06T04:52:09Z- handleDepth()- Depth_Keller msg: 1730868729.72 epoch sec; seqNo:2506; depth 50.31 2024-11-06T04:52:09Z- handleDVL() - DVL msg epochMillisec: 1730868729.72 2024-11-06T04:52:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:09Z- handleDvl() - RDI_Pathfinder msg: 1730868729.72 sec; seqNo:919; #beams:0 2024-11-06T04:52:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , -0.01 2024-11-06T04:52:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868729.73 epoch sec; seqNo:2506; 4061720.57 north; 594773.19 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:52:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868725.69 2024-11-06T04:52:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:09Z- waiting for 1730868729.69, time is 1730868729.73 2024-11-06T04:52:09Z- LcmTrn::updateTrn() >>>> pose time 1730868729.73, meas time 1730868729.72 <<<< 2024-11-06T04:52:09Z- LcmTrn::updateTrn() equating pose 1730868729.72 and meas times 1730868729.72 2024-11-06T04:52:09Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868729.72 2024-11-06T04:52:09Z- TerrainNav::motionUpdate() - currEstimate = (4061721, 594773.19, 50.31) 2024-11-06T04:52:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:52:09Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:52:09Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:52:09Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:52:09Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:52:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:52:09Z- variances([0], [2], and psi_berg) = 0.778626 0.547418 0.000000 2024-11-06T04:52:09Z- publishEstimatesDecoder() - at 1730868729.72 sec 2024-11-06T04:52:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:52:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061664.62 594776.88 54.07 2024-11-06T04:52:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061663.96 594778.34 53.60 lat/lon: 36.70 -121.94 2024-11-06T04:52:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.78 0.55 0.67 0.00 2024-11-06T04:52:10Z- AHRS_M2 msg: 1730868730.11 epoch sec; seqNo:2344 2024-11-06T04:52:10Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.53 psi 2024-11-06T04:52:10Z- handleDepth()- Depth_Keller msg: 1730868730.12 epoch sec; seqNo:2507; depth 50.31 2024-11-06T04:52:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868730.14 epoch sec; seqNo:2507; 4061720.57 north; 594773.19 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:52:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868729.72 2024-11-06T04:52:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:10Z- waiting for 1730868733.72, time is 1730868730.14 2024-11-06T04:52:10Z- AHRS_M2 msg: 1730868730.53 epoch sec; seqNo:2345 2024-11-06T04:52:10Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.53 psi 2024-11-06T04:52:10Z- handleDepth()- Depth_Keller msg: 1730868730.53 epoch sec; seqNo:2508; depth 50.30 2024-11-06T04:52:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868730.54 epoch sec; seqNo:2508; 4061720.59 north; 594774.41 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:52:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868729.72 2024-11-06T04:52:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:10Z- waiting for 1730868733.72, time is 1730868730.54 2024-11-06T04:52:10Z- AHRS_M2 msg: 1730868730.93 epoch sec; seqNo:2346 2024-11-06T04:52:10Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.53 psi 2024-11-06T04:52:10Z- handleDepth()- Depth_Keller msg: 1730868730.93 epoch sec; seqNo:2509; depth 50.30 2024-11-06T04:52:10Z- handleDVL() - DVL msg epochMillisec: 1730868730.95 2024-11-06T04:52:10Z- handleDvl() - RDI_Pathfinder msg: 1730868730.95 sec; seqNo:920; #beams:4 2024-11-06T04:52:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.08 , 30.41 , 36.36 , 36.02 2024-11-06T04:52:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , 0.02 2024-11-06T04:52:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868731.00 epoch sec; seqNo:2509; 4061720.59 north; 594774.41 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:52:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868729.72 2024-11-06T04:52:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:11Z- waiting for 1730868733.72, time is 1730868731.00 2024-11-06T04:52:11Z- AHRS_M2 msg: 1730868731.33 epoch sec; seqNo:2347 2024-11-06T04:52:11Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.53 psi 2024-11-06T04:52:11Z- handleDepth()- Depth_Keller msg: 1730868731.34 epoch sec; seqNo:2510; depth 50.30 2024-11-06T04:52:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868731.36 epoch sec; seqNo:2510; 4061720.59 north; 594774.41 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:52:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868729.72 2024-11-06T04:52:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:11Z- waiting for 1730868733.72, time is 1730868731.36 2024-11-06T04:52:11Z- handleDepth()- Depth_Keller msg: 1730868731.75 epoch sec; seqNo:2511; depth 50.31 2024-11-06T04:52:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868731.77 epoch sec; seqNo:2511; 4061720.60 north; 594775.63 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:52:11Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:52:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868729.72 2024-11-06T04:52:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:11Z- waiting for 1730868733.72, time is 1730868731.77 2024-11-06T04:52:12Z- AHRS_M2 msg: 1730868732.14 epoch sec; seqNo:2348 2024-11-06T04:52:12Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.52 psi 2024-11-06T04:52:12Z- handleDepth()- Depth_Keller msg: 1730868732.14 epoch sec; seqNo:2512; depth 50.29 2024-11-06T04:52:12Z- handleDVL() - DVL msg epochMillisec: 1730868732.14 2024-11-06T04:52:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:12Z- handleDvl() - RDI_Pathfinder msg: 1730868732.14 sec; seqNo:921; #beams:0 2024-11-06T04:52:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.07 , 0.02 2024-11-06T04:52:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868732.15 epoch sec; seqNo:2512; 4061720.98 north; 594775.62 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:52:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868729.72 2024-11-06T04:52:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:12Z- waiting for 1730868733.72, time is 1730868732.15 2024-11-06T04:52:12Z- AHRS_M2 msg: 1730868732.55 epoch sec; seqNo:2349 2024-11-06T04:52:12Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.52 psi 2024-11-06T04:52:12Z- handleDepth()- Depth_Keller msg: 1730868732.56 epoch sec; seqNo:2513; depth 50.29 2024-11-06T04:52:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868732.57 epoch sec; seqNo:2513; 4061720.98 north; 594775.62 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:52:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868729.72 2024-11-06T04:52:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:12Z- waiting for 1730868733.72, time is 1730868732.57 2024-11-06T04:52:12Z- AHRS_M2 msg: 1730868732.94 epoch sec; seqNo:2350 2024-11-06T04:52:12Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.52 psi 2024-11-06T04:52:12Z- handleDepth()- Depth_Keller msg: 1730868732.95 epoch sec; seqNo:2514; depth 50.30 2024-11-06T04:52:12Z- handleDVL() - DVL msg epochMillisec: 1730868732.97 2024-11-06T04:52:12Z- handleDvl() - RDI_Pathfinder msg: 1730868732.97 sec; seqNo:922; #beams:4 2024-11-06T04:52:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.40 , 30.75 , 36.19 , 36.02 2024-11-06T04:52:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.06 , 0.05 2024-11-06T04:52:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868733.00 epoch sec; seqNo:2514; 4061720.98 north; 594775.62 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:52:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868729.72 2024-11-06T04:52:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:13Z- waiting for 1730868733.72, time is 1730868733.00 2024-11-06T04:52:13Z- AHRS_M2 msg: 1730868733.35 epoch sec; seqNo:2351 2024-11-06T04:52:13Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.52 psi 2024-11-06T04:52:13Z- handleDepth()- Depth_Keller msg: 1730868733.36 epoch sec; seqNo:2515; depth 50.28 2024-11-06T04:52:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868733.37 epoch sec; seqNo:2515; 4061720.99 north; 594776.85 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T04:52:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868729.72 2024-11-06T04:52:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:13Z- waiting for 1730868733.72, time is 1730868733.37 2024-11-06T04:52:13Z- AHRS_M2 msg: 1730868733.75 epoch sec; seqNo:2352 2024-11-06T04:52:13Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.52 psi 2024-11-06T04:52:13Z- handleDepth()- Depth_Keller msg: 1730868733.76 epoch sec; seqNo:2516; depth 50.27 2024-11-06T04:52:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868733.77 epoch sec; seqNo:2516; 4061720.99 north; 594776.85 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T04:52:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868729.72 2024-11-06T04:52:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:13Z- waiting for 1730868733.72, time is 1730868733.77 2024-11-06T04:52:13Z- LcmTrn::updateTrn() >>>> pose time 1730868733.77, meas time 1730868732.97 <<<< 2024-11-06T04:52:13Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868733.77 2024-11-06T04:52:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868732.97 sec, ping # 0; waiting for INS data more recent than 1730868729.72... 2024-11-06T04:52:13Z- TerrainNav::motionUpdate() - currEstimate = (4061721, 594776.85, 50.27) 2024-11-06T04:52:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:52:13Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:52:13Z- TNavPF::Projecting Measurements 2024-11-06T04:52:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:52:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:52:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:52:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:52:13Z- TNavFilter::4 good beams. 2024-11-06T04:52:13Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:52:13Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:52:13Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:52:13Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:52:13Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:52:13Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:52:13Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:52:13Z- TerrainMapOctree: (vehN, vehE ) = (4061720.91, 594776.12). 2024-11-06T04:52:13Z- TerrainMapOctree: Min Distance = 39.10. 2024-11-06T04:52:13Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:52:14Z- Forcing Subcloud Comparison 2024-11-06T04:52:14Z- Weighting particles with subcloud comparison 2024-11-06T04:52:14Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:14Z- meanExpectedMeasDiff: -0.031957 delta_rms_squared: 1.485894 alpha: 0.986719 2024-11-06T04:52:14Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:14Z- meanExpectedMeasDiff: -2.695572 delta_rms_squared: 1.186117 alpha: 0.983418 2024-11-06T04:52:14Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:14Z- meanExpectedMeasDiff: -0.393780 delta_rms_squared: 2.374339 alpha: 0.991647 2024-11-06T04:52:14Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:14Z- meanExpectedMeasDiff: 0.405537 delta_rms_squared: 1.197128 alpha: 0.983568 2024-11-06T04:52:14Z- Subcloud NIS: 0.477579 2024-11-06T04:52:14Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:52:14Z- TNavPF:: sumWeights = 0.223109 2024-11-06T04:52:14Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:52:14Z- TNavPF::First Term of Map Covariance Matrix is 0.15 2024-11-06T04:52:14Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T04:52:14Z- TerrainNav::Resampling particle filter... 2024-11-06T04:52:14Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868732.97 sec. 2024-11-06T04:52:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:52:14Z- variances([0], [2], and psi_berg) = 0.829664 0.631840 0.000000 2024-11-06T04:52:14Z- publishEstimatesDecoder() - at 1730868733.77 sec 2024-11-06T04:52:14Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:52:14Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061665.06 594780.64 53.83 2024-11-06T04:52:14Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061664.48 594781.95 53.90 lat/lon: 36.70 -121.94 2024-11-06T04:52:14Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.83 0.63 0.62 0.00 2024-11-06T04:52:14Z- AHRS_M2 msg: 1730868734.15 epoch sec; seqNo:2353 2024-11-06T04:52:14Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.52 psi 2024-11-06T04:52:14Z- handleDepth()- Depth_Keller msg: 1730868734.17 epoch sec; seqNo:2517; depth 50.26 2024-11-06T04:52:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868734.19 epoch sec; seqNo:2517; 4061720.99 north; 594776.85 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:52:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868733.77 2024-11-06T04:52:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:14Z- waiting for 1730868737.77, time is 1730868734.19 2024-11-06T04:52:14Z- AHRS_M2 msg: 1730868734.57 epoch sec; seqNo:2354 2024-11-06T04:52:14Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 1.52 psi 2024-11-06T04:52:14Z- handleDepth()- Depth_Keller msg: 1730868734.58 epoch sec; seqNo:2518; depth 50.26 2024-11-06T04:52:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868734.59 epoch sec; seqNo:2518; 4061721.01 north; 594778.07 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:52:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868733.77 2024-11-06T04:52:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:14Z- waiting for 1730868737.77, time is 1730868734.59 2024-11-06T04:52:14Z- AHRS_M2 msg: 1730868734.96 epoch sec; seqNo:2355 2024-11-06T04:52:14Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.52 psi 2024-11-06T04:52:14Z- handleDepth()- Depth_Keller msg: 1730868734.97 epoch sec; seqNo:2519; depth 50.24 2024-11-06T04:52:15Z- handleDVL() - DVL msg epochMillisec: 1730868734.99 2024-11-06T04:52:15Z- handleDvl() - RDI_Pathfinder msg: 1730868734.99 sec; seqNo:923; #beams:4 2024-11-06T04:52:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.02 , 31.26 , 35.68 , 35.85 2024-11-06T04:52:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.02 , 0.03 2024-11-06T04:52:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868735.01 epoch sec; seqNo:2519; 4061721.01 north; 594778.07 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:52:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868733.77 2024-11-06T04:52:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:15Z- waiting for 1730868737.77, time is 1730868735.01 2024-11-06T04:52:15Z- AHRS_M2 msg: 1730868735.37 epoch sec; seqNo:2356 2024-11-06T04:52:15Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:52:15Z- handleDepth()- Depth_Keller msg: 1730868735.37 epoch sec; seqNo:2520; depth 50.23 2024-11-06T04:52:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868735.39 epoch sec; seqNo:2520; 4061721.01 north; 594778.07 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T04:52:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868733.77 2024-11-06T04:52:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:15Z- waiting for 1730868737.77, time is 1730868735.39 2024-11-06T04:52:15Z- AHRS_M2 msg: 1730868735.77 epoch sec; seqNo:2357 2024-11-06T04:52:15Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:52:15Z- handleDepth()- Depth_Keller msg: 1730868735.78 epoch sec; seqNo:2521; depth 50.21 2024-11-06T04:52:15Z- handleDVL() - DVL msg epochMillisec: 1730868735.78 2024-11-06T04:52:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:15Z- handleDvl() - RDI_Pathfinder msg: 1730868735.78 sec; seqNo:924; #beams:0 2024-11-06T04:52:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.02 , 0.03 2024-11-06T04:52:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868735.79 epoch sec; seqNo:2521; 4061721.39 north; 594778.06 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T04:52:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868733.77 2024-11-06T04:52:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:15Z- waiting for 1730868737.77, time is 1730868735.79 2024-11-06T04:52:16Z- AHRS_M2 msg: 1730868736.18 epoch sec; seqNo:2358 2024-11-06T04:52:16Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:52:16Z- handleDepth()- Depth_Keller msg: 1730868736.18 epoch sec; seqNo:2522; depth 50.20 2024-11-06T04:52:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868736.19 epoch sec; seqNo:2522; 4061721.40 north; 594779.28 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:52:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868733.77 2024-11-06T04:52:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:16Z- waiting for 1730868737.77, time is 1730868736.19 2024-11-06T04:52:16Z- AHRS_M2 msg: 1730868736.58 epoch sec; seqNo:2359 2024-11-06T04:52:16Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:52:16Z- handleDepth()- Depth_Keller msg: 1730868736.59 epoch sec; seqNo:2523; depth 50.18 2024-11-06T04:52:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868736.62 epoch sec; seqNo:2523; 4061721.40 north; 594779.28 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T04:52:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868733.77 2024-11-06T04:52:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:16Z- waiting for 1730868737.77, time is 1730868736.62 2024-11-06T04:52:16Z- AHRS_M2 msg: 1730868736.99 epoch sec; seqNo:2360 2024-11-06T04:52:16Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:52:17Z- handleDepth()- Depth_Keller msg: 1730868736.99 epoch sec; seqNo:2524; depth 50.16 2024-11-06T04:52:17Z- handleDVL() - DVL msg epochMillisec: 1730868737.03 2024-11-06T04:52:17Z- handleDvl() - RDI_Pathfinder msg: 1730868737.03 sec; seqNo:925; #beams:4 2024-11-06T04:52:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.34 , 31.26 , 35.51 , 35.85 2024-11-06T04:52:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.02 , -0.02 2024-11-06T04:52:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868737.04 epoch sec; seqNo:2524; 4061721.40 north; 594779.28 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T04:52:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868733.77 2024-11-06T04:52:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:17Z- waiting for 1730868737.77, time is 1730868737.04 2024-11-06T04:52:17Z- AHRS_M2 msg: 1730868737.39 epoch sec; seqNo:2361 2024-11-06T04:52:17Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:52:17Z- handleDepth()- Depth_Keller msg: 1730868737.39 epoch sec; seqNo:2525; depth 50.14 2024-11-06T04:52:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868737.41 epoch sec; seqNo:2525; 4061721.41 north; 594780.50 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:52:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868733.77 2024-11-06T04:52:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:17Z- waiting for 1730868737.77, time is 1730868737.41 2024-11-06T04:52:17Z- AHRS_M2 msg: 1730868737.79 epoch sec; seqNo:2362 2024-11-06T04:52:17Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:52:17Z- handleDepth()- Depth_Keller msg: 1730868737.80 epoch sec; seqNo:2526; depth 50.12 2024-11-06T04:52:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868737.82 epoch sec; seqNo:2526; 4061721.41 north; 594780.50 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T04:52:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868733.77 2024-11-06T04:52:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:17Z- waiting for 1730868737.77, time is 1730868737.82 2024-11-06T04:52:17Z- LcmTrn::updateTrn() >>>> pose time 1730868737.82, meas time 1730868737.03 <<<< 2024-11-06T04:52:17Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868737.82 2024-11-06T04:52:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868737.03 sec, ping # 0; waiting for INS data more recent than 1730868733.77... 2024-11-06T04:52:17Z- TerrainNav::motionUpdate() - currEstimate = (4061721, 594780.50, 50.12) 2024-11-06T04:52:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:52:17Z- TerrainNav:: SubcloudNIS is 0.48, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:52:17Z- TNavPF::Projecting Measurements 2024-11-06T04:52:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:52:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:52:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:52:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:52:17Z- TNavFilter::4 good beams. 2024-11-06T04:52:17Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:52:17Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:52:17Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:52:17Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:52:17Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:52:17Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:52:17Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:52:17Z- TerrainMapOctree: (vehN, vehE ) = (4061721.33, 594779.79). 2024-11-06T04:52:17Z- TerrainMapOctree: Min Distance = 41.34. 2024-11-06T04:52:17Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:52:18Z- Forcing Subcloud Comparison 2024-11-06T04:52:18Z- Weighting particles with subcloud comparison 2024-11-06T04:52:18Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:18Z- meanExpectedMeasDiff: -2.910149 delta_rms_squared: 1.421789 alpha: 0.986128 2024-11-06T04:52:18Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:18Z- meanExpectedMeasDiff: -1.971151 delta_rms_squared: 1.021960 alpha: 0.980805 2024-11-06T04:52:18Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:18Z- meanExpectedMeasDiff: -1.810456 delta_rms_squared: 2.184078 alpha: 0.990926 2024-11-06T04:52:18Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:18Z- meanExpectedMeasDiff: 0.512699 delta_rms_squared: 1.135547 alpha: 0.982692 2024-11-06T04:52:18Z- Subcloud NIS: 0.494646 2024-11-06T04:52:18Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:52:18Z- TNavPF:: sumWeights = 0.167441 2024-11-06T04:52:18Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:52:18Z- TNavPF::First Term of Map Covariance Matrix is 0.54 2024-11-06T04:52:18Z- TNavPF::Calculated NIS Value : 0.07 numBeams normalized NIS: 0.02 2024-11-06T04:52:18Z- TerrainNav::Resampling particle filter... 2024-11-06T04:52:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868737.03 sec. 2024-11-06T04:52:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:52:18Z- variances([0], [2], and psi_berg) = 0.805756 0.585877 0.000000 2024-11-06T04:52:18Z- publishEstimatesDecoder() - at 1730868737.82 sec 2024-11-06T04:52:18Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:52:18Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061665.52 594784.34 54.57 2024-11-06T04:52:18Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061664.97 594785.56 54.49 lat/lon: 36.70 -121.94 2024-11-06T04:52:18Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.81 0.59 0.61 0.00 2024-11-06T04:52:18Z- AHRS_M2 msg: 1730868738.20 epoch sec; seqNo:2363 2024-11-06T04:52:18Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.54 psi 2024-11-06T04:52:18Z- handleDepth()- Depth_Keller msg: 1730868738.20 epoch sec; seqNo:2527; depth 50.08 2024-11-06T04:52:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868738.22 epoch sec; seqNo:2527; 4061721.41 north; 594780.50 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T04:52:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868737.82 2024-11-06T04:52:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:18Z- waiting for 1730868741.82, time is 1730868738.22 2024-11-06T04:52:18Z- AHRS_M2 msg: 1730868738.61 epoch sec; seqNo:2364 2024-11-06T04:52:18Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.54 psi 2024-11-06T04:52:18Z- handleDepth()- Depth_Keller msg: 1730868738.61 epoch sec; seqNo:2528; depth 50.07 2024-11-06T04:52:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868738.62 epoch sec; seqNo:2528; 4061721.43 north; 594781.72 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T04:52:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868737.82 2024-11-06T04:52:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:18Z- waiting for 1730868741.82, time is 1730868738.62 2024-11-06T04:52:19Z- handleDepth()- Depth_Keller msg: 1730868739.02 epoch sec; seqNo:2529; depth 50.05 2024-11-06T04:52:19Z- handleDVL() - DVL msg epochMillisec: 1730868739.05 2024-11-06T04:52:19Z- handleDvl() - RDI_Pathfinder msg: 1730868739.05 sec; seqNo:926; #beams:4 2024-11-06T04:52:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.51 , 31.94 , 33.98 , 35.68 2024-11-06T04:52:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , -0.07 2024-11-06T04:52:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868739.07 epoch sec; seqNo:2529; 4061721.43 north; 594781.72 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T04:52:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868737.82 2024-11-06T04:52:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:19Z- waiting for 1730868741.82, time is 1730868739.07 2024-11-06T04:52:19Z- AHRS_M2 msg: 1730868739.41 epoch sec; seqNo:2365 2024-11-06T04:52:19Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.54 psi 2024-11-06T04:52:19Z- handleDepth()- Depth_Keller msg: 1730868739.42 epoch sec; seqNo:2530; depth 50.00 2024-11-06T04:52:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868739.44 epoch sec; seqNo:2530; 4061721.43 north; 594781.72 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T04:52:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868737.82 2024-11-06T04:52:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:19Z- waiting for 1730868741.82, time is 1730868739.44 2024-11-06T04:52:19Z- AHRS_M2 msg: 1730868739.82 epoch sec; seqNo:2366 2024-11-06T04:52:19Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.54 psi 2024-11-06T04:52:19Z- handleDepth()- Depth_Keller msg: 1730868739.83 epoch sec; seqNo:2531; depth 50.00 2024-11-06T04:52:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868739.84 epoch sec; seqNo:2531; 4061721.44 north; 594782.94 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T04:52:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868737.82 2024-11-06T04:52:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:19Z- waiting for 1730868741.82, time is 1730868739.84 2024-11-06T04:52:20Z- AHRS_M2 msg: 1730868740.22 epoch sec; seqNo:2367 2024-11-06T04:52:20Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.54 psi 2024-11-06T04:52:20Z- handleDepth()- Depth_Keller msg: 1730868740.22 epoch sec; seqNo:2532; depth 49.98 2024-11-06T04:52:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868740.23 epoch sec; seqNo:2532; 4061721.44 north; 594782.94 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T04:52:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868737.82 2024-11-06T04:52:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:20Z- waiting for 1730868741.82, time is 1730868740.23 2024-11-06T04:52:20Z- AHRS_M2 msg: 1730868740.62 epoch sec; seqNo:2368 2024-11-06T04:52:20Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.54 psi 2024-11-06T04:52:20Z- handleDepth()- Depth_Keller msg: 1730868740.63 epoch sec; seqNo:2533; depth 49.96 2024-11-06T04:52:20Z- handleDVL() - DVL msg epochMillisec: 1730868740.63 2024-11-06T04:52:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:20Z- handleDvl() - RDI_Pathfinder msg: 1730868740.63 sec; seqNo:927; #beams:0 2024-11-06T04:52:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , -0.07 2024-11-06T04:52:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868740.64 epoch sec; seqNo:2533; 4061721.44 north; 594782.94 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:52:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868737.82 2024-11-06T04:52:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:20Z- waiting for 1730868741.82, time is 1730868740.64 2024-11-06T04:52:21Z- AHRS_M2 msg: 1730868741.02 epoch sec; seqNo:2369 2024-11-06T04:52:21Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.54 psi 2024-11-06T04:52:21Z- handleDepth()- Depth_Keller msg: 1730868741.04 epoch sec; seqNo:2534; depth 49.94 2024-11-06T04:52:21Z- handleDVL() - DVL msg epochMillisec: 1730868741.06 2024-11-06T04:52:21Z- handleDvl() - RDI_Pathfinder msg: 1730868741.06 sec; seqNo:928; #beams:4 2024-11-06T04:52:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.93 , 32.42 , 33.64 , 35.17 2024-11-06T04:52:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.04 , -0.06 2024-11-06T04:52:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868741.07 epoch sec; seqNo:2534; 4061721.44 north; 594782.94 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T04:52:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868737.82 2024-11-06T04:52:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:21Z- waiting for 1730868741.82, time is 1730868741.07 2024-11-06T04:52:21Z- AHRS_M2 msg: 1730868741.43 epoch sec; seqNo:2370 2024-11-06T04:52:21Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:52:21Z- handleDepth()- Depth_Keller msg: 1730868741.43 epoch sec; seqNo:2535; depth 49.92 2024-11-06T04:52:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868741.45 epoch sec; seqNo:2535; 4061721.45 north; 594784.16 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:52:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868737.82 2024-11-06T04:52:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:21Z- waiting for 1730868741.82, time is 1730868741.45 2024-11-06T04:52:21Z- AHRS_M2 msg: 1730868741.84 epoch sec; seqNo:2371 2024-11-06T04:52:21Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:52:21Z- handleDepth()- Depth_Keller msg: 1730868741.84 epoch sec; seqNo:2536; depth 49.89 2024-11-06T04:52:21Z- handleDVL() - DVL msg epochMillisec: 1730868741.84 2024-11-06T04:52:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:21Z- handleDvl() - RDI_Pathfinder msg: 1730868741.84 sec; seqNo:929; #beams:0 2024-11-06T04:52:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.04 , -0.06 2024-11-06T04:52:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868741.85 epoch sec; seqNo:2536; 4061721.45 north; 594784.16 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:52:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868737.82 2024-11-06T04:52:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:22Z- waiting for 1730868741.82, time is 1730868741.85 2024-11-06T04:52:22Z- LcmTrn::updateTrn() >>>> pose time 1730868741.85, meas time 1730868741.84 <<<< 2024-11-06T04:52:22Z- LcmTrn::updateTrn() equating pose 1730868741.84 and meas times 1730868741.84 2024-11-06T04:52:22Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868741.84 2024-11-06T04:52:22Z- TerrainNav::motionUpdate() - currEstimate = (4061721, 594784.16, 49.89) 2024-11-06T04:52:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:52:22Z- TerrainNav:: SubcloudNIS is 0.49, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:52:22Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:52:22Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:52:22Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:52:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:52:22Z- variances([0], [2], and psi_berg) = 0.808981 0.587312 0.000000 2024-11-06T04:52:22Z- publishEstimatesDecoder() - at 1730868741.84 sec 2024-11-06T04:52:22Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:52:22Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061665.50 594788.00 54.64 2024-11-06T04:52:22Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061665.01 594789.22 54.25 lat/lon: 36.70 -121.94 2024-11-06T04:52:22Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.81 0.59 0.86 0.00 2024-11-06T04:52:22Z- AHRS_M2 msg: 1730868742.24 epoch sec; seqNo:2372 2024-11-06T04:52:22Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:52:22Z- handleDepth()- Depth_Keller msg: 1730868742.25 epoch sec; seqNo:2537; depth 49.89 2024-11-06T04:52:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868742.26 epoch sec; seqNo:2537; 4061721.45 north; 594784.16 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:52:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868741.84 2024-11-06T04:52:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:22Z- waiting for 1730868745.84, time is 1730868742.26 2024-11-06T04:52:22Z- AHRS_M2 msg: 1730868742.64 epoch sec; seqNo:2373 2024-11-06T04:52:22Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:52:22Z- handleDepth()- Depth_Keller msg: 1730868742.65 epoch sec; seqNo:2538; depth 49.87 2024-11-06T04:52:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868742.67 epoch sec; seqNo:2538; 4061721.45 north; 594784.16 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:52:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868741.84 2024-11-06T04:52:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:22Z- waiting for 1730868745.84, time is 1730868742.67 2024-11-06T04:52:23Z- AHRS_M2 msg: 1730868743.05 epoch sec; seqNo:2374 2024-11-06T04:52:23Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:52:23Z- handleDepth()- Depth_Keller msg: 1730868743.05 epoch sec; seqNo:2539; depth 49.86 2024-11-06T04:52:23Z- handleDVL() - DVL msg epochMillisec: 1730868743.08 2024-11-06T04:52:23Z- handleDvl() - RDI_Pathfinder msg: 1730868743.08 sec; seqNo:930; #beams:4 2024-11-06T04:52:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.93 , 32.87 , 33.64 , 34.10 2024-11-06T04:52:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.07 , 0.00 2024-11-06T04:52:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868743.09 epoch sec; seqNo:2539; 4061721.47 north; 594785.38 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:52:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868741.84 2024-11-06T04:52:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:23Z- waiting for 1730868745.84, time is 1730868743.09 2024-11-06T04:52:23Z- AHRS_M2 msg: 1730868743.45 epoch sec; seqNo:2375 2024-11-06T04:52:23Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.53 psi 2024-11-06T04:52:23Z- handleDepth()- Depth_Keller msg: 1730868743.45 epoch sec; seqNo:2540; depth 49.84 2024-11-06T04:52:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868743.47 epoch sec; seqNo:2540; 4061721.47 north; 594785.38 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T04:52:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868741.84 2024-11-06T04:52:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:23Z- waiting for 1730868745.84, time is 1730868743.47 2024-11-06T04:52:23Z- AHRS_M2 msg: 1730868743.85 epoch sec; seqNo:2376 2024-11-06T04:52:23Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.52 psi 2024-11-06T04:52:23Z- handleDepth()- Depth_Keller msg: 1730868743.86 epoch sec; seqNo:2541; depth 49.84 2024-11-06T04:52:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868743.87 epoch sec; seqNo:2541; 4061721.47 north; 594785.38 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T04:52:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868741.84 2024-11-06T04:52:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:24Z- waiting for 1730868745.84, time is 1730868743.87 2024-11-06T04:52:24Z- AHRS_M2 msg: 1730868744.26 epoch sec; seqNo:2377 2024-11-06T04:52:24Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.52 psi 2024-11-06T04:52:24Z- handleDepth()- Depth_Keller msg: 1730868744.26 epoch sec; seqNo:2542; depth 49.81 2024-11-06T04:52:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868744.28 epoch sec; seqNo:2542; 4061721.85 north; 594785.38 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:52:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868741.84 2024-11-06T04:52:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:24Z- waiting for 1730868745.84, time is 1730868744.28 2024-11-06T04:52:24Z- AHRS_M2 msg: 1730868744.66 epoch sec; seqNo:2378 2024-11-06T04:52:24Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.52 psi 2024-11-06T04:52:24Z- handleDepth()- Depth_Keller msg: 1730868744.67 epoch sec; seqNo:2543; depth 49.82 2024-11-06T04:52:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868744.71 epoch sec; seqNo:2543; 4061721.86 north; 594786.60 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:52:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868741.84 2024-11-06T04:52:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:24Z- waiting for 1730868745.84, time is 1730868744.71 2024-11-06T04:52:25Z- AHRS_M2 msg: 1730868745.06 epoch sec; seqNo:2379 2024-11-06T04:52:25Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 1.52 psi 2024-11-06T04:52:25Z- handleDepth()- Depth_Keller msg: 1730868745.07 epoch sec; seqNo:2544; depth 49.81 2024-11-06T04:52:25Z- handleDVL() - DVL msg epochMillisec: 1730868745.09 2024-11-06T04:52:25Z- handleDvl() - RDI_Pathfinder msg: 1730868745.09 sec; seqNo:931; #beams:4 2024-11-06T04:52:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 35.63 , 33.33 , 34.56 , 34.25 2024-11-06T04:52:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.07 , 0.04 2024-11-06T04:52:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868745.12 epoch sec; seqNo:2544; 4061721.86 north; 594786.60 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:52:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868741.84 2024-11-06T04:52:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:25Z- waiting for 1730868745.84, time is 1730868745.12 2024-11-06T04:52:25Z- AHRS_M2 msg: 1730868745.47 epoch sec; seqNo:2380 2024-11-06T04:52:25Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.52 psi 2024-11-06T04:52:25Z- handleDepth()- Depth_Keller msg: 1730868745.47 epoch sec; seqNo:2545; depth 49.80 2024-11-06T04:52:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868745.49 epoch sec; seqNo:2545; 4061721.86 north; 594786.60 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T04:52:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868741.84 2024-11-06T04:52:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:25Z- waiting for 1730868745.84, time is 1730868745.49 2024-11-06T04:52:25Z- AHRS_M2 msg: 1730868745.87 epoch sec; seqNo:2381 2024-11-06T04:52:25Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.52 psi 2024-11-06T04:52:25Z- handleDepth()- Depth_Keller msg: 1730868745.88 epoch sec; seqNo:2546; depth 49.79 2024-11-06T04:52:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868745.89 epoch sec; seqNo:2546; 4061721.87 north; 594787.82 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:52:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868741.84 2024-11-06T04:52:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:26Z- waiting for 1730868745.84, time is 1730868745.89 2024-11-06T04:52:26Z- LcmTrn::updateTrn() >>>> pose time 1730868745.89, meas time 1730868745.09 <<<< 2024-11-06T04:52:26Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868745.89 2024-11-06T04:52:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868745.09 sec, ping # 0; waiting for INS data more recent than 1730868741.84... 2024-11-06T04:52:26Z- TerrainNav::motionUpdate() - currEstimate = (4061722, 594787.82, 49.79) 2024-11-06T04:52:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:52:26Z- TerrainNav:: SubcloudNIS is 0.49, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:52:26Z- TNavPF::Projecting Measurements 2024-11-06T04:52:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:52:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:52:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:52:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:52:26Z- TNavFilter::4 good beams. 2024-11-06T04:52:26Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:52:26Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:52:26Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:52:26Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:52:26Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:52:26Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:52:26Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:52:26Z- TerrainMapOctree: (vehN, vehE ) = (4061721.79, 594787.10). 2024-11-06T04:52:26Z- TerrainMapOctree: Min Distance = 46.61. 2024-11-06T04:52:26Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:52:26Z- Forcing Subcloud Comparison 2024-11-06T04:52:26Z- Weighting particles with subcloud comparison 2024-11-06T04:52:26Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:26Z- meanExpectedMeasDiff: -3.462720 delta_rms_squared: 1.806606 alpha: 0.989051 2024-11-06T04:52:26Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:26Z- meanExpectedMeasDiff: 0.922726 delta_rms_squared: 1.936660 alpha: 0.989778 2024-11-06T04:52:26Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:26Z- meanExpectedMeasDiff: -5.664748 delta_rms_squared: 2.623057 alpha: 0.992433 2024-11-06T04:52:26Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:26Z- meanExpectedMeasDiff: 0.391318 delta_rms_squared: 1.466528 alpha: 0.986546 2024-11-06T04:52:26Z- Subcloud NIS: 0.521102 2024-11-06T04:52:26Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:52:26Z- TNavPF:: sumWeights = 0.007178 2024-11-06T04:52:26Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:52:26Z- TNavPF::First Term of Map Covariance Matrix is 0.04 2024-11-06T04:52:26Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:52:26Z- TerrainNav::Resampling particle filter... 2024-11-06T04:52:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868745.09 sec. 2024-11-06T04:52:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:52:26Z- variances([0], [2], and psi_berg) = 0.968019 0.678994 0.000000 2024-11-06T04:52:26Z- publishEstimatesDecoder() - at 1730868745.89 sec 2024-11-06T04:52:26Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:52:26Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061665.98 594791.66 54.95 2024-11-06T04:52:26Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061665.76 594792.46 55.17 lat/lon: 36.70 -121.94 2024-11-06T04:52:26Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.97 0.68 0.65 0.00 2024-11-06T04:52:26Z- AHRS_M2 msg: 1730868746.28 epoch sec; seqNo:2382 2024-11-06T04:52:26Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.52 psi 2024-11-06T04:52:26Z- handleDepth()- Depth_Keller msg: 1730868746.29 epoch sec; seqNo:2547; depth 49.78 2024-11-06T04:52:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868746.32 epoch sec; seqNo:2547; 4061721.87 north; 594787.82 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:52:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868745.89 2024-11-06T04:52:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:26Z- waiting for 1730868749.89, time is 1730868746.32 2024-11-06T04:52:26Z- AHRS_M2 msg: 1730868746.73 epoch sec; seqNo:2383 2024-11-06T04:52:26Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.52 psi 2024-11-06T04:52:26Z- handleDepth()- Depth_Keller msg: 1730868746.75 epoch sec; seqNo:2548; depth 49.77 2024-11-06T04:52:26Z- handleDVL() - DVL msg epochMillisec: 1730868746.79 2024-11-06T04:52:26Z- handleDvl() - RDI_Pathfinder msg: 1730868746.79 sec; seqNo:932; #beams:4 2024-11-06T04:52:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.02 , 33.30 , 34.32 , 35.00 2024-11-06T04:52:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.04 2024-11-06T04:52:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868746.80 epoch sec; seqNo:2548; 4061721.87 north; 594787.82 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T04:52:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868745.89 2024-11-06T04:52:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:26Z- waiting for 1730868749.89, time is 1730868746.80 2024-11-06T04:52:27Z- AHRS_M2 msg: 1730868747.09 epoch sec; seqNo:2384 2024-11-06T04:52:27Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.52 psi 2024-11-06T04:52:27Z- handleDepth()- Depth_Keller msg: 1730868747.10 epoch sec; seqNo:2549; depth 49.77 2024-11-06T04:52:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868747.11 epoch sec; seqNo:2549; 4061721.87 north; 594787.82 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T04:52:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868745.89 2024-11-06T04:52:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:27Z- waiting for 1730868749.89, time is 1730868747.11 2024-11-06T04:52:27Z- AHRS_M2 msg: 1730868747.49 epoch sec; seqNo:2385 2024-11-06T04:52:27Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.52 psi 2024-11-06T04:52:27Z- handleDepth()- Depth_Keller msg: 1730868747.49 epoch sec; seqNo:2550; depth 49.75 2024-11-06T04:52:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868747.51 epoch sec; seqNo:2550; 4061721.89 north; 594789.04 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:52:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868745.89 2024-11-06T04:52:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:27Z- waiting for 1730868749.89, time is 1730868747.51 2024-11-06T04:52:27Z- AHRS_M2 msg: 1730868747.90 epoch sec; seqNo:2386 2024-11-06T04:52:27Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.52 psi 2024-11-06T04:52:27Z- handleDepth()- Depth_Keller msg: 1730868747.90 epoch sec; seqNo:2551; depth 49.74 2024-11-06T04:52:27Z- handleDVL() - DVL msg epochMillisec: 1730868747.91 2024-11-06T04:52:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:27Z- handleDvl() - RDI_Pathfinder msg: 1730868747.91 sec; seqNo:933; #beams:0 2024-11-06T04:52:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.04 2024-11-06T04:52:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868747.93 epoch sec; seqNo:2551; 4061721.89 north; 594789.04 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T04:52:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868745.89 2024-11-06T04:52:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:28Z- waiting for 1730868749.89, time is 1730868747.93 2024-11-06T04:52:28Z- AHRS_M2 msg: 1730868748.30 epoch sec; seqNo:2387 2024-11-06T04:52:28Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.52 psi 2024-11-06T04:52:28Z- handleDepth()- Depth_Keller msg: 1730868748.30 epoch sec; seqNo:2552; depth 49.75 2024-11-06T04:52:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868748.31 epoch sec; seqNo:2552; 4061722.27 north; 594789.04 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:52:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868745.89 2024-11-06T04:52:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:28Z- waiting for 1730868749.89, time is 1730868748.31 2024-11-06T04:52:28Z- AHRS_M2 msg: 1730868748.74 epoch sec; seqNo:2388 2024-11-06T04:52:28Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.53 psi 2024-11-06T04:52:28Z- handleDepth()- Depth_Keller msg: 1730868748.75 epoch sec; seqNo:2553; depth 49.73 2024-11-06T04:52:28Z- handleDVL() - DVL msg epochMillisec: 1730868748.78 2024-11-06T04:52:28Z- handleDvl() - RDI_Pathfinder msg: 1730868748.78 sec; seqNo:934; #beams:4 2024-11-06T04:52:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.70 , 33.64 , 34.15 , 35.51 2024-11-06T04:52:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.00 , 0.02 2024-11-06T04:52:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868748.79 epoch sec; seqNo:2553; 4061722.28 north; 594790.26 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T04:52:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868745.89 2024-11-06T04:52:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:28Z- waiting for 1730868749.89, time is 1730868748.79 2024-11-06T04:52:29Z- AHRS_M2 msg: 1730868749.11 epoch sec; seqNo:2389 2024-11-06T04:52:29Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.53 psi 2024-11-06T04:52:29Z- handleDepth()- Depth_Keller msg: 1730868749.12 epoch sec; seqNo:2554; depth 49.70 2024-11-06T04:52:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868749.13 epoch sec; seqNo:2554; 4061722.28 north; 594790.26 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T04:52:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868745.89 2024-11-06T04:52:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:29Z- waiting for 1730868749.89, time is 1730868749.13 2024-11-06T04:52:29Z- AHRS_M2 msg: 1730868749.51 epoch sec; seqNo:2390 2024-11-06T04:52:29Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.53 psi 2024-11-06T04:52:29Z- handleDepth()- Depth_Keller msg: 1730868749.52 epoch sec; seqNo:2555; depth 49.71 2024-11-06T04:52:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868749.54 epoch sec; seqNo:2555; 4061722.28 north; 594790.26 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T04:52:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868745.89 2024-11-06T04:52:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:29Z- waiting for 1730868749.89, time is 1730868749.54 2024-11-06T04:52:29Z- AHRS_M2 msg: 1730868749.91 epoch sec; seqNo:2391 2024-11-06T04:52:29Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.53 psi 2024-11-06T04:52:29Z- handleDepth()- Depth_Keller msg: 1730868749.92 epoch sec; seqNo:2556; depth 49.68 2024-11-06T04:52:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868749.92 epoch sec; seqNo:2556; 4061722.28 north; 594790.26 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T04:52:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868745.89 2024-11-06T04:52:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:30Z- waiting for 1730868749.89, time is 1730868749.92 2024-11-06T04:52:30Z- LcmTrn::updateTrn() >>>> pose time 1730868749.92, meas time 1730868748.78 <<<< 2024-11-06T04:52:30Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868749.92 2024-11-06T04:52:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868748.78 sec, ping # 0; waiting for INS data more recent than 1730868745.89... 2024-11-06T04:52:30Z- TerrainNav::motionUpdate() - currEstimate = (4061722, 594790.26, 49.68) 2024-11-06T04:52:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:52:30Z- TerrainNav:: SubcloudNIS is 0.52, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:52:30Z- TNavPF::Projecting Measurements 2024-11-06T04:52:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:52:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:52:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:52:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:52:30Z- TNavFilter::4 good beams. 2024-11-06T04:52:30Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:52:30Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:52:30Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:52:30Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:52:30Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:52:30Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:52:30Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:52:30Z- TerrainMapOctree: (vehN, vehE ) = (4061722.16, 594789.56). 2024-11-06T04:52:30Z- TerrainMapOctree: Min Distance = 48.37. 2024-11-06T04:52:30Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:52:30Z- Forcing Subcloud Comparison 2024-11-06T04:52:30Z- Weighting particles with subcloud comparison 2024-11-06T04:52:30Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:30Z- meanExpectedMeasDiff: -1.017286 delta_rms_squared: 1.547584 alpha: 0.987242 2024-11-06T04:52:30Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:30Z- meanExpectedMeasDiff: 2.840198 delta_rms_squared: 1.162120 alpha: 0.983081 2024-11-06T04:52:30Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:30Z- meanExpectedMeasDiff: -4.922304 delta_rms_squared: 1.937452 alpha: 0.989783 2024-11-06T04:52:30Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:30Z- meanExpectedMeasDiff: 2.550909 delta_rms_squared: 1.173463 alpha: 0.983242 2024-11-06T04:52:30Z- Subcloud NIS: 0.571134 2024-11-06T04:52:30Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:52:30Z- TNavPF:: sumWeights = 0.004178 2024-11-06T04:52:30Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:52:30Z- TNavPF::First Term of Map Covariance Matrix is 0.01 2024-11-06T04:52:30Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:52:30Z- TerrainNav::Resampling particle filter... 2024-11-06T04:52:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868748.78 sec. 2024-11-06T04:52:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:52:30Z- variances([0], [2], and psi_berg) = 0.952201 0.691569 0.000000 2024-11-06T04:52:30Z- publishEstimatesDecoder() - at 1730868749.92 sec 2024-11-06T04:52:30Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:52:30Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061666.37 594794.10 55.22 2024-11-06T04:52:30Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061666.96 594794.31 55.07 lat/lon: 36.70 -121.94 2024-11-06T04:52:30Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.95 0.69 0.68 0.00 2024-11-06T04:52:30Z- AHRS_M2 msg: 1730868750.32 epoch sec; seqNo:2392 2024-11-06T04:52:30Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.52 psi 2024-11-06T04:52:30Z- handleDepth()- Depth_Keller msg: 1730868750.32 epoch sec; seqNo:2557; depth 49.68 2024-11-06T04:52:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868750.34 epoch sec; seqNo:2557; 4061722.29 north; 594791.48 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T04:52:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868749.92 2024-11-06T04:52:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:30Z- waiting for 1730868753.92, time is 1730868750.34 2024-11-06T04:52:30Z- AHRS_M2 msg: 1730868750.75 epoch sec; seqNo:2393 2024-11-06T04:52:30Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.52 psi 2024-11-06T04:52:30Z- handleDepth()- Depth_Keller msg: 1730868750.76 epoch sec; seqNo:2558; depth 49.66 2024-11-06T04:52:30Z- handleDVL() - DVL msg epochMillisec: 1730868750.78 2024-11-06T04:52:30Z- handleDvl() - RDI_Pathfinder msg: 1730868750.78 sec; seqNo:935; #beams:4 2024-11-06T04:52:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.90 , 33.47 , 34.15 , 35.85 2024-11-06T04:52:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.01 , -0.01 2024-11-06T04:52:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868750.81 epoch sec; seqNo:2558; 4061722.29 north; 594791.48 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:52:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868749.92 2024-11-06T04:52:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:30Z- waiting for 1730868753.92, time is 1730868750.81 2024-11-06T04:52:31Z- handleDepth()- Depth_Keller msg: 1730868751.15 epoch sec; seqNo:2559; depth 49.64 2024-11-06T04:52:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868751.16 epoch sec; seqNo:2559; 4061722.29 north; 594791.48 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:52:31Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:52:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868749.92 2024-11-06T04:52:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:31Z- waiting for 1730868753.92, time is 1730868751.16 2024-11-06T04:52:31Z- AHRS_M2 msg: 1730868751.53 epoch sec; seqNo:2394 2024-11-06T04:52:31Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.52 psi 2024-11-06T04:52:31Z- handleDepth()- Depth_Keller msg: 1730868751.54 epoch sec; seqNo:2560; depth 49.62 2024-11-06T04:52:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868751.55 epoch sec; seqNo:2560; 4061722.31 north; 594792.70 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:52:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868749.92 2024-11-06T04:52:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:31Z- waiting for 1730868753.92, time is 1730868751.55 2024-11-06T04:52:31Z- AHRS_M2 msg: 1730868751.93 epoch sec; seqNo:2395 2024-11-06T04:52:31Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.52 psi 2024-11-06T04:52:31Z- handleDepth()- Depth_Keller msg: 1730868751.94 epoch sec; seqNo:2561; depth 49.61 2024-11-06T04:52:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868751.95 epoch sec; seqNo:2561; 4061722.31 north; 594792.70 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:52:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868749.92 2024-11-06T04:52:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:32Z- waiting for 1730868753.92, time is 1730868751.95 2024-11-06T04:52:32Z- AHRS_M2 msg: 1730868752.34 epoch sec; seqNo:2396 2024-11-06T04:52:32Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.53 psi 2024-11-06T04:52:32Z- handleDepth()- Depth_Keller msg: 1730868752.34 epoch sec; seqNo:2562; depth 49.61 2024-11-06T04:52:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868752.35 epoch sec; seqNo:2562; 4061722.31 north; 594792.70 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:52:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868749.92 2024-11-06T04:52:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:32Z- waiting for 1730868753.92, time is 1730868752.35 2024-11-06T04:52:32Z- AHRS_M2 msg: 1730868752.77 epoch sec; seqNo:2397 2024-11-06T04:52:32Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.53 psi 2024-11-06T04:52:32Z- handleDepth()- Depth_Keller msg: 1730868752.77 epoch sec; seqNo:2563; depth 49.58 2024-11-06T04:52:32Z- handleDVL() - DVL msg epochMillisec: 1730868752.79 2024-11-06T04:52:32Z- handleDvl() - RDI_Pathfinder msg: 1730868752.79 sec; seqNo:936; #beams:4 2024-11-06T04:52:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.56 , 33.81 , 34.49 , 36.19 2024-11-06T04:52:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , -0.04 2024-11-06T04:52:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868752.82 epoch sec; seqNo:2563; 4061722.32 north; 594793.92 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:52:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868749.92 2024-11-06T04:52:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:32Z- waiting for 1730868753.92, time is 1730868752.82 2024-11-06T04:52:33Z- AHRS_M2 msg: 1730868753.14 epoch sec; seqNo:2398 2024-11-06T04:52:33Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.53 psi 2024-11-06T04:52:33Z- handleDepth()- Depth_Keller msg: 1730868753.15 epoch sec; seqNo:2564; depth 49.56 2024-11-06T04:52:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868753.17 epoch sec; seqNo:2564; 4061722.32 north; 594793.92 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:52:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868749.92 2024-11-06T04:52:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:33Z- waiting for 1730868753.92, time is 1730868753.17 2024-11-06T04:52:33Z- AHRS_M2 msg: 1730868753.55 epoch sec; seqNo:2399 2024-11-06T04:52:33Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.53 psi 2024-11-06T04:52:33Z- handleDepth()- Depth_Keller msg: 1730868753.56 epoch sec; seqNo:2565; depth 49.56 2024-11-06T04:52:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868753.56 epoch sec; seqNo:2565; 4061722.32 north; 594793.92 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:52:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868749.92 2024-11-06T04:52:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:33Z- waiting for 1730868753.92, time is 1730868753.56 2024-11-06T04:52:33Z- AHRS_M2 msg: 1730868753.95 epoch sec; seqNo:2400 2024-11-06T04:52:33Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.53 psi 2024-11-06T04:52:33Z- handleDepth()- Depth_Keller msg: 1730868753.96 epoch sec; seqNo:2566; depth 49.56 2024-11-06T04:52:33Z- handleDVL() - DVL msg epochMillisec: 1730868753.96 2024-11-06T04:52:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:33Z- handleDvl() - RDI_Pathfinder msg: 1730868753.96 sec; seqNo:937; #beams:0 2024-11-06T04:52:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , -0.04 2024-11-06T04:52:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868753.97 epoch sec; seqNo:2566; 4061722.32 north; 594793.92 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:52:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868749.92 2024-11-06T04:52:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:34Z- waiting for 1730868753.92, time is 1730868753.97 2024-11-06T04:52:34Z- LcmTrn::updateTrn() >>>> pose time 1730868753.97, meas time 1730868753.96 <<<< 2024-11-06T04:52:34Z- LcmTrn::updateTrn() equating pose 1730868753.96 and meas times 1730868753.96 2024-11-06T04:52:34Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868753.96 2024-11-06T04:52:34Z- TerrainNav::motionUpdate() - currEstimate = (4061722, 594793.92, 49.56) 2024-11-06T04:52:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:52:34Z- TerrainNav:: SubcloudNIS is 0.57, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:52:34Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:52:34Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:52:34Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:52:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:52:34Z- variances([0], [2], and psi_berg) = 0.956157 0.691994 0.000000 2024-11-06T04:52:34Z- publishEstimatesDecoder() - at 1730868753.96 sec 2024-11-06T04:52:34Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:52:34Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061666.42 594797.74 55.20 2024-11-06T04:52:34Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061667.00 594797.97 54.94 lat/lon: 36.70 -121.94 2024-11-06T04:52:34Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.96 0.69 0.92 0.00 2024-11-06T04:52:34Z- AHRS_M2 msg: 1730868754.36 epoch sec; seqNo:2401 2024-11-06T04:52:34Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.52 psi 2024-11-06T04:52:34Z- handleDepth()- Depth_Keller msg: 1730868754.37 epoch sec; seqNo:2567; depth 49.53 2024-11-06T04:52:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868754.38 epoch sec; seqNo:2567; 4061722.33 north; 594795.14 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T04:52:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868753.96 2024-11-06T04:52:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:34Z- waiting for 1730868757.96, time is 1730868754.38 2024-11-06T04:52:34Z- AHRS_M2 msg: 1730868754.76 epoch sec; seqNo:2402 2024-11-06T04:52:34Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.52 psi 2024-11-06T04:52:34Z- handleDepth()- Depth_Keller msg: 1730868754.77 epoch sec; seqNo:2568; depth 49.52 2024-11-06T04:52:34Z- handleDVL() - DVL msg epochMillisec: 1730868754.81 2024-11-06T04:52:34Z- handleDvl() - RDI_Pathfinder msg: 1730868754.81 sec; seqNo:938; #beams:4 2024-11-06T04:52:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.74 , 32.96 , 33.98 , 36.36 2024-11-06T04:52:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , -0.05 2024-11-06T04:52:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868754.85 epoch sec; seqNo:2568; 4061722.33 north; 594795.14 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:52:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868753.96 2024-11-06T04:52:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:34Z- waiting for 1730868757.96, time is 1730868754.85 2024-11-06T04:52:35Z- handleDepth()- Depth_Keller msg: 1730868755.18 epoch sec; seqNo:2569; depth 49.51 2024-11-06T04:52:35Z- handleDVL() - DVL msg epochMillisec: 1730868755.18 2024-11-06T04:52:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:35Z- handleDvl() - RDI_Pathfinder msg: 1730868755.18 sec; seqNo:939; #beams:0 2024-11-06T04:52:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , -0.05 2024-11-06T04:52:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868755.20 epoch sec; seqNo:2569; 4061722.33 north; 594795.14 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T04:52:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868753.96 2024-11-06T04:52:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:35Z- waiting for 1730868757.96, time is 1730868755.20 2024-11-06T04:52:35Z- AHRS_M2 msg: 1730868755.57 epoch sec; seqNo:2403 2024-11-06T04:52:35Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.52 psi 2024-11-06T04:52:35Z- handleDepth()- Depth_Keller msg: 1730868755.58 epoch sec; seqNo:2570; depth 49.50 2024-11-06T04:52:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868755.59 epoch sec; seqNo:2570; 4061722.35 north; 594796.36 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T04:52:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868753.96 2024-11-06T04:52:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:35Z- waiting for 1730868757.96, time is 1730868755.59 2024-11-06T04:52:35Z- AHRS_M2 msg: 1730868755.97 epoch sec; seqNo:2404 2024-11-06T04:52:35Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 1.52 psi 2024-11-06T04:52:35Z- handleDepth()- Depth_Keller msg: 1730868755.99 epoch sec; seqNo:2571; depth 49.51 2024-11-06T04:52:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868755.99 epoch sec; seqNo:2571; 4061722.35 north; 594796.36 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T04:52:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868753.96 2024-11-06T04:52:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:36Z- waiting for 1730868757.96, time is 1730868755.99 2024-11-06T04:52:36Z- AHRS_M2 msg: 1730868756.38 epoch sec; seqNo:2405 2024-11-06T04:52:36Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.52 psi 2024-11-06T04:52:36Z- handleDepth()- Depth_Keller msg: 1730868756.38 epoch sec; seqNo:2572; depth 49.49 2024-11-06T04:52:36Z- handleDVL() - DVL msg epochMillisec: 1730868756.38 2024-11-06T04:52:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:36Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:36Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:36Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:36Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:36Z- handleDvl() - RDI_Pathfinder msg: 1730868756.38 sec; seqNo:940; #beams:0 2024-11-06T04:52:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , -0.05 2024-11-06T04:52:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868756.41 epoch sec; seqNo:2572; 4061722.35 north; 594796.36 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:52:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868753.96 2024-11-06T04:52:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:36Z- waiting for 1730868757.96, time is 1730868756.41 2024-11-06T04:52:36Z- AHRS_M2 msg: 1730868756.78 epoch sec; seqNo:2406 2024-11-06T04:52:36Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.52 psi 2024-11-06T04:52:36Z- handleDepth()- Depth_Keller msg: 1730868756.78 epoch sec; seqNo:2573; depth 49.49 2024-11-06T04:52:36Z- handleDVL() - DVL msg epochMillisec: 1730868756.81 2024-11-06T04:52:36Z- handleDvl() - RDI_Pathfinder msg: 1730868756.81 sec; seqNo:941; #beams:4 2024-11-06T04:52:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.40 , 33.13 , 33.64 , 36.53 2024-11-06T04:52:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.05 , -0.01 2024-11-06T04:52:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868756.85 epoch sec; seqNo:2573; 4061722.73 north; 594796.35 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:52:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868753.96 2024-11-06T04:52:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:36Z- waiting for 1730868757.96, time is 1730868756.85 2024-11-06T04:52:37Z- AHRS_M2 msg: 1730868757.19 epoch sec; seqNo:2407 2024-11-06T04:52:37Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.51 psi 2024-11-06T04:52:37Z- handleDepth()- Depth_Keller msg: 1730868757.20 epoch sec; seqNo:2574; depth 49.49 2024-11-06T04:52:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868757.22 epoch sec; seqNo:2574; 4061722.74 north; 594797.57 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:52:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868753.96 2024-11-06T04:52:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:37Z- waiting for 1730868757.96, time is 1730868757.22 2024-11-06T04:52:37Z- AHRS_M2 msg: 1730868757.59 epoch sec; seqNo:2408 2024-11-06T04:52:37Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 1.51 psi 2024-11-06T04:52:37Z- handleDepth()- Depth_Keller msg: 1730868757.60 epoch sec; seqNo:2575; depth 49.49 2024-11-06T04:52:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868757.61 epoch sec; seqNo:2575; 4061722.74 north; 594797.57 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:52:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868753.96 2024-11-06T04:52:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:37Z- waiting for 1730868757.96, time is 1730868757.61 2024-11-06T04:52:38Z- AHRS_M2 msg: 1730868757.99 epoch sec; seqNo:2409 2024-11-06T04:52:38Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.51 psi 2024-11-06T04:52:38Z- handleDepth()- Depth_Keller msg: 1730868758.00 epoch sec; seqNo:2576; depth 49.49 2024-11-06T04:52:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868758.01 epoch sec; seqNo:2576; 4061722.74 north; 594797.57 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:52:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868753.96 2024-11-06T04:52:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:38Z- waiting for 1730868757.96, time is 1730868758.01 2024-11-06T04:52:38Z- LcmTrn::updateTrn() >>>> pose time 1730868758.01, meas time 1730868756.81 <<<< 2024-11-06T04:52:38Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868758.01 2024-11-06T04:52:38Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868756.81 sec, ping # 0; waiting for INS data more recent than 1730868753.96... 2024-11-06T04:52:38Z- TerrainNav::motionUpdate() - currEstimate = (4061723, 594797.57, 49.49) 2024-11-06T04:52:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:52:38Z- TerrainNav:: SubcloudNIS is 0.57, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:52:38Z- TNavPF::Projecting Measurements 2024-11-06T04:52:38Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:52:38Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:52:38Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:52:38Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:52:38Z- TNavFilter::4 good beams. 2024-11-06T04:52:38Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:52:38Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:52:38Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:52:38Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:52:38Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:52:38Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:52:38Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:52:38Z- TerrainMapOctree: (vehN, vehE ) = (4061722.62, 594796.49). 2024-11-06T04:52:38Z- TerrainMapOctree: Min Distance = 53.96. 2024-11-06T04:52:38Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:52:38Z- Forcing Subcloud Comparison 2024-11-06T04:52:38Z- Weighting particles with subcloud comparison 2024-11-06T04:52:38Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:38Z- meanExpectedMeasDiff: 1.474695 delta_rms_squared: 1.977796 alpha: 0.989989 2024-11-06T04:52:38Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:38Z- meanExpectedMeasDiff: 1.043556 delta_rms_squared: 1.426747 alpha: 0.986176 2024-11-06T04:52:38Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:38Z- meanExpectedMeasDiff: -7.265933 delta_rms_squared: 2.166756 alpha: 0.990854 2024-11-06T04:52:38Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:38Z- meanExpectedMeasDiff: 2.881859 delta_rms_squared: 1.593560 alpha: 0.987605 2024-11-06T04:52:38Z- Subcloud NIS: 0.636144 2024-11-06T04:52:38Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:52:38Z- TNavPF:: sumWeights = 0.000129 2024-11-06T04:52:38Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:52:38Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T04:52:38Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:52:38Z- TerrainNav::Resampling particle filter... 2024-11-06T04:52:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868756.81 sec. 2024-11-06T04:52:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:52:38Z- variances([0], [2], and psi_berg) = 0.732733 0.421780 0.000000 2024-11-06T04:52:38Z- publishEstimatesDecoder() - at 1730868758.01 sec 2024-11-06T04:52:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:52:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061666.84 594801.45 54.64 2024-11-06T04:52:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061667.67 594801.33 55.22 lat/lon: 36.70 -121.94 2024-11-06T04:52:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.73 0.42 0.75 0.00 2024-11-06T04:52:38Z- AHRS_M2 msg: 1730868758.39 epoch sec; seqNo:2410 2024-11-06T04:52:38Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.51 psi 2024-11-06T04:52:38Z- handleDepth()- Depth_Keller msg: 1730868758.41 epoch sec; seqNo:2577; depth 49.49 2024-11-06T04:52:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868758.43 epoch sec; seqNo:2577; 4061722.74 north; 594797.57 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:52:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868758.01 2024-11-06T04:52:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:38Z- waiting for 1730868762.01, time is 1730868758.43 2024-11-06T04:52:38Z- AHRS_M2 msg: 1730868758.80 epoch sec; seqNo:2411 2024-11-06T04:52:38Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T04:52:38Z- handleDepth()- Depth_Keller msg: 1730868758.81 epoch sec; seqNo:2578; depth 49.48 2024-11-06T04:52:38Z- handleDVL() - DVL msg epochMillisec: 1730868758.84 2024-11-06T04:52:38Z- handleDvl() - RDI_Pathfinder msg: 1730868758.84 sec; seqNo:942; #beams:4 2024-11-06T04:52:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.74 , 33.13 , 33.81 , 36.70 2024-11-06T04:52:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , 0.04 2024-11-06T04:52:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868758.86 epoch sec; seqNo:2578; 4061722.75 north; 594798.79 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T04:52:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868758.01 2024-11-06T04:52:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:38Z- waiting for 1730868762.01, time is 1730868758.86 2024-11-06T04:52:39Z- AHRS_M2 msg: 1730868759.21 epoch sec; seqNo:2412 2024-11-06T04:52:39Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T04:52:39Z- handleDepth()- Depth_Keller msg: 1730868759.22 epoch sec; seqNo:2579; depth 49.50 2024-11-06T04:52:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868759.23 epoch sec; seqNo:2579; 4061722.75 north; 594798.79 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T04:52:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868758.01 2024-11-06T04:52:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:39Z- waiting for 1730868762.01, time is 1730868759.23 2024-11-06T04:52:39Z- AHRS_M2 msg: 1730868759.61 epoch sec; seqNo:2413 2024-11-06T04:52:39Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T04:52:39Z- handleDepth()- Depth_Keller msg: 1730868759.61 epoch sec; seqNo:2580; depth 49.48 2024-11-06T04:52:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868759.63 epoch sec; seqNo:2580; 4061722.75 north; 594798.79 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T04:52:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868758.01 2024-11-06T04:52:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:39Z- waiting for 1730868762.01, time is 1730868759.63 2024-11-06T04:52:40Z- AHRS_M2 msg: 1730868760.01 epoch sec; seqNo:2414 2024-11-06T04:52:40Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T04:52:40Z- handleDepth()- Depth_Keller msg: 1730868760.02 epoch sec; seqNo:2581; depth 49.49 2024-11-06T04:52:40Z- handleDVL() - DVL msg epochMillisec: 1730868760.02 2024-11-06T04:52:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:40Z- handleDvl() - RDI_Pathfinder msg: 1730868760.02 sec; seqNo:943; #beams:0 2024-11-06T04:52:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , 0.04 2024-11-06T04:52:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868760.04 epoch sec; seqNo:2581; 4061722.77 north; 594800.01 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:52:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868758.01 2024-11-06T04:52:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:40Z- waiting for 1730868762.01, time is 1730868760.04 2024-11-06T04:52:40Z- AHRS_M2 msg: 1730868760.42 epoch sec; seqNo:2415 2024-11-06T04:52:40Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T04:52:40Z- handleDepth()- Depth_Keller msg: 1730868760.42 epoch sec; seqNo:2582; depth 49.49 2024-11-06T04:52:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868760.43 epoch sec; seqNo:2582; 4061723.14 north; 594800.01 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:52:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868758.01 2024-11-06T04:52:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:40Z- waiting for 1730868762.01, time is 1730868760.43 2024-11-06T04:52:40Z- AHRS_M2 msg: 1730868760.83 epoch sec; seqNo:2416 2024-11-06T04:52:40Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T04:52:40Z- handleDepth()- Depth_Keller msg: 1730868760.84 epoch sec; seqNo:2583; depth 49.50 2024-11-06T04:52:40Z- handleDVL() - DVL msg epochMillisec: 1730868760.86 2024-11-06T04:52:40Z- handleDvl() - RDI_Pathfinder msg: 1730868760.86 sec; seqNo:944; #beams:4 2024-11-06T04:52:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.07 , 33.13 , 33.98 , 37.21 2024-11-06T04:52:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.06 2024-11-06T04:52:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868760.87 epoch sec; seqNo:2583; 4061723.14 north; 594800.01 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T04:52:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868758.01 2024-11-06T04:52:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:40Z- waiting for 1730868762.01, time is 1730868760.87 2024-11-06T04:52:41Z- AHRS_M2 msg: 1730868761.22 epoch sec; seqNo:2417 2024-11-06T04:52:41Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T04:52:41Z- handleDepth()- Depth_Keller msg: 1730868761.23 epoch sec; seqNo:2584; depth 49.51 2024-11-06T04:52:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868761.24 epoch sec; seqNo:2584; 4061723.14 north; 594800.01 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T04:52:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868758.01 2024-11-06T04:52:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:41Z- waiting for 1730868762.01, time is 1730868761.24 2024-11-06T04:52:41Z- AHRS_M2 msg: 1730868761.63 epoch sec; seqNo:2418 2024-11-06T04:52:41Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:52:41Z- handleDepth()- Depth_Keller msg: 1730868761.63 epoch sec; seqNo:2585; depth 49.50 2024-11-06T04:52:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868761.64 epoch sec; seqNo:2585; 4061723.16 north; 594801.23 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T04:52:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868758.01 2024-11-06T04:52:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:41Z- waiting for 1730868762.01, time is 1730868761.64 2024-11-06T04:52:42Z- AHRS_M2 msg: 1730868762.03 epoch sec; seqNo:2419 2024-11-06T04:52:42Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:52:42Z- handleDepth()- Depth_Keller msg: 1730868762.04 epoch sec; seqNo:2586; depth 49.50 2024-11-06T04:52:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868762.06 epoch sec; seqNo:2586; 4061723.16 north; 594801.23 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T04:52:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868758.01 2024-11-06T04:52:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:42Z- waiting for 1730868762.01, time is 1730868762.06 2024-11-06T04:52:42Z- LcmTrn::updateTrn() >>>> pose time 1730868762.06, meas time 1730868760.86 <<<< 2024-11-06T04:52:42Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868762.06 2024-11-06T04:52:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868760.86 sec, ping # 0; waiting for INS data more recent than 1730868758.01... 2024-11-06T04:52:42Z- TerrainNav::motionUpdate() - currEstimate = (4061723, 594801.23, 49.50) 2024-11-06T04:52:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:52:42Z- TerrainNav:: SubcloudNIS is 0.64, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:52:42Z- TNavPF::Projecting Measurements 2024-11-06T04:52:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:52:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:52:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:52:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:52:42Z- TNavFilter::4 good beams. 2024-11-06T04:52:42Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:52:42Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:52:42Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:52:42Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:52:42Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:52:42Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:52:42Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:52:42Z- TerrainMapOctree: (vehN, vehE ) = (4061723.03, 594800.14). 2024-11-06T04:52:42Z- TerrainMapOctree: Min Distance = 56.94. 2024-11-06T04:52:42Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:52:42Z- Forcing Subcloud Comparison 2024-11-06T04:52:42Z- Weighting particles with subcloud comparison 2024-11-06T04:52:42Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:42Z- meanExpectedMeasDiff: 1.514831 delta_rms_squared: 1.637543 alpha: 0.987934 2024-11-06T04:52:42Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:42Z- meanExpectedMeasDiff: -0.362279 delta_rms_squared: 2.671600 alpha: 0.992569 2024-11-06T04:52:42Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:42Z- meanExpectedMeasDiff: -7.113159 delta_rms_squared: 1.545587 alpha: 0.987225 2024-11-06T04:52:42Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:42Z- meanExpectedMeasDiff: 3.737080 delta_rms_squared: 1.267381 alpha: 0.984465 2024-11-06T04:52:42Z- Subcloud NIS: 0.701002 2024-11-06T04:52:42Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:52:42Z- TNavPF:: sumWeights = 0.000081 2024-11-06T04:52:42Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:52:42Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T04:52:42Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:52:42Z- TerrainNav::Resampling particle filter... 2024-11-06T04:52:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868760.86 sec. 2024-11-06T04:52:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:52:42Z- variances([0], [2], and psi_berg) = 0.969476 0.570945 0.000000 2024-11-06T04:52:42Z- publishEstimatesDecoder() - at 1730868762.06 sec 2024-11-06T04:52:42Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:52:42Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061667.23 594805.17 56.39 2024-11-06T04:52:42Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061667.92 594805.13 55.72 lat/lon: 36.70 -121.94 2024-11-06T04:52:42Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.97 0.57 0.79 0.00 2024-11-06T04:52:42Z- AHRS_M2 msg: 1730868762.45 epoch sec; seqNo:2420 2024-11-06T04:52:42Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:52:42Z- handleDepth()- Depth_Keller msg: 1730868762.46 epoch sec; seqNo:2587; depth 49.50 2024-11-06T04:52:42Z- handleDVL() - DVL msg epochMillisec: 1730868762.47 2024-11-06T04:52:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:42Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:42Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:42Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:42Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:42Z- handleDvl() - RDI_Pathfinder msg: 1730868762.47 sec; seqNo:945; #beams:0 2024-11-06T04:52:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.02 , 0.06 2024-11-06T04:52:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868762.49 epoch sec; seqNo:2587; 4061723.16 north; 594801.23 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T04:52:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868762.06 2024-11-06T04:52:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:42Z- waiting for 1730868766.06, time is 1730868762.49 2024-11-06T04:52:42Z- AHRS_M2 msg: 1730868762.84 epoch sec; seqNo:2421 2024-11-06T04:52:42Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T04:52:42Z- handleDepth()- Depth_Keller msg: 1730868762.86 epoch sec; seqNo:2588; depth 49.51 2024-11-06T04:52:42Z- handleDVL() - DVL msg epochMillisec: 1730868762.88 2024-11-06T04:52:42Z- handleDvl() - RDI_Pathfinder msg: 1730868762.88 sec; seqNo:946; #beams:4 2024-11-06T04:52:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.07 , 33.47 , 33.98 , 37.54 2024-11-06T04:52:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , 0.01 2024-11-06T04:52:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868762.89 epoch sec; seqNo:2588; 4061723.17 north; 594802.45 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T04:52:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868762.06 2024-11-06T04:52:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:42Z- waiting for 1730868766.06, time is 1730868762.89 2024-11-06T04:52:43Z- AHRS_M2 msg: 1730868763.24 epoch sec; seqNo:2422 2024-11-06T04:52:43Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T04:52:43Z- handleDepth()- Depth_Keller msg: 1730868763.25 epoch sec; seqNo:2589; depth 49.50 2024-11-06T04:52:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868763.27 epoch sec; seqNo:2589; 4061723.17 north; 594802.45 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T04:52:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868762.06 2024-11-06T04:52:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:43Z- waiting for 1730868766.06, time is 1730868763.27 2024-11-06T04:52:43Z- AHRS_M2 msg: 1730868763.65 epoch sec; seqNo:2423 2024-11-06T04:52:43Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.50 psi 2024-11-06T04:52:43Z- handleDepth()- Depth_Keller msg: 1730868763.65 epoch sec; seqNo:2590; depth 49.51 2024-11-06T04:52:43Z- handleDVL() - DVL msg epochMillisec: 1730868763.66 2024-11-06T04:52:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:43Z- handleDvl() - RDI_Pathfinder msg: 1730868763.66 sec; seqNo:947; #beams:0 2024-11-06T04:52:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , 0.01 2024-11-06T04:52:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868763.68 epoch sec; seqNo:2590; 4061723.17 north; 594802.45 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T04:52:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868762.06 2024-11-06T04:52:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:43Z- waiting for 1730868766.06, time is 1730868763.68 2024-11-06T04:52:44Z- AHRS_M2 msg: 1730868764.06 epoch sec; seqNo:2424 2024-11-06T04:52:44Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.50 psi 2024-11-06T04:52:44Z- handleDepth()- Depth_Keller msg: 1730868764.06 epoch sec; seqNo:2591; depth 49.52 2024-11-06T04:52:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868764.07 epoch sec; seqNo:2591; 4061723.17 north; 594802.45 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:52:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868762.06 2024-11-06T04:52:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:44Z- waiting for 1730868766.06, time is 1730868764.07 2024-11-06T04:52:44Z- AHRS_M2 msg: 1730868764.45 epoch sec; seqNo:2425 2024-11-06T04:52:44Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:52:44Z- handleDepth()- Depth_Keller msg: 1730868764.46 epoch sec; seqNo:2592; depth 49.49 2024-11-06T04:52:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868764.48 epoch sec; seqNo:2592; 4061723.19 north; 594803.67 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:52:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868762.06 2024-11-06T04:52:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:44Z- waiting for 1730868766.06, time is 1730868764.48 2024-11-06T04:52:44Z- AHRS_M2 msg: 1730868764.86 epoch sec; seqNo:2426 2024-11-06T04:52:44Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:52:44Z- handleDepth()- Depth_Keller msg: 1730868764.87 epoch sec; seqNo:2593; depth 49.50 2024-11-06T04:52:44Z- handleDVL() - DVL msg epochMillisec: 1730868764.90 2024-11-06T04:52:44Z- handleDvl() - RDI_Pathfinder msg: 1730868764.90 sec; seqNo:948; #beams:4 2024-11-06T04:52:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.40 , 33.47 , 33.81 , 37.21 2024-11-06T04:52:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.04 2024-11-06T04:52:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868764.91 epoch sec; seqNo:2593; 4061723.19 north; 594803.67 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T04:52:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868762.06 2024-11-06T04:52:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:45Z- waiting for 1730868766.06, time is 1730868764.91 2024-11-06T04:52:45Z- AHRS_M2 msg: 1730868765.27 epoch sec; seqNo:2427 2024-11-06T04:52:45Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.50 psi 2024-11-06T04:52:45Z- handleDepth()- Depth_Keller msg: 1730868765.27 epoch sec; seqNo:2594; depth 49.49 2024-11-06T04:52:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868765.30 epoch sec; seqNo:2594; 4061723.19 north; 594803.67 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:52:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868762.06 2024-11-06T04:52:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:45Z- waiting for 1730868766.06, time is 1730868765.30 2024-11-06T04:52:45Z- AHRS_M2 msg: 1730868765.68 epoch sec; seqNo:2428 2024-11-06T04:52:45Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.50 psi 2024-11-06T04:52:45Z- handleDepth()- Depth_Keller msg: 1730868765.68 epoch sec; seqNo:2595; depth 49.48 2024-11-06T04:52:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868765.69 epoch sec; seqNo:2595; 4061723.20 north; 594804.89 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T04:52:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868762.06 2024-11-06T04:52:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:45Z- waiting for 1730868766.06, time is 1730868765.69 2024-11-06T04:52:46Z- AHRS_M2 msg: 1730868766.07 epoch sec; seqNo:2429 2024-11-06T04:52:46Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:52:46Z- handleDepth()- Depth_Keller msg: 1730868766.08 epoch sec; seqNo:2596; depth 49.49 2024-11-06T04:52:46Z- handleDVL() - DVL msg epochMillisec: 1730868766.08 2024-11-06T04:52:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:46Z- handleDvl() - RDI_Pathfinder msg: 1730868766.08 sec; seqNo:949; #beams:0 2024-11-06T04:52:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.04 2024-11-06T04:52:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868766.10 epoch sec; seqNo:2596; 4061723.58 north; 594804.89 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:52:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868762.06 2024-11-06T04:52:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:46Z- waiting for 1730868766.06, time is 1730868766.10 2024-11-06T04:52:46Z- LcmTrn::updateTrn() >>>> pose time 1730868766.10, meas time 1730868766.08 <<<< 2024-11-06T04:52:46Z- LcmTrn::updateTrn() equating pose 1730868766.08 and meas times 1730868766.08 2024-11-06T04:52:46Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868766.08 2024-11-06T04:52:46Z- TerrainNav::motionUpdate() - currEstimate = (4061724, 594804.89, 49.49) 2024-11-06T04:52:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:52:46Z- TerrainNav:: SubcloudNIS is 0.70, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:52:46Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:52:46Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:52:46Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:52:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:52:46Z- variances([0], [2], and psi_berg) = 0.971828 0.573239 0.000000 2024-11-06T04:52:46Z- publishEstimatesDecoder() - at 1730868766.08 sec 2024-11-06T04:52:46Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:52:46Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061667.63 594808.86 57.14 2024-11-06T04:52:46Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061668.34 594808.78 55.72 lat/lon: 36.70 -121.94 2024-11-06T04:52:46Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.97 0.57 1.04 0.00 2024-11-06T04:52:46Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:52:46Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:52:46Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:52:46Z- AHRS_M2 msg: 1730868766.47 epoch sec; seqNo:2430 2024-11-06T04:52:46Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:52:46Z- handleDepth()- Depth_Keller msg: 1730868766.49 epoch sec; seqNo:2597; depth 49.48 2024-11-06T04:52:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868766.51 epoch sec; seqNo:2597; 4061723.58 north; 594804.89 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T04:52:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868766.08 2024-11-06T04:52:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:46Z- waiting for 1730868770.08, time is 1730868766.51 2024-11-06T04:52:46Z- AHRS_M2 msg: 1730868766.88 epoch sec; seqNo:2431 2024-11-06T04:52:46Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:52:46Z- handleDepth()- Depth_Keller msg: 1730868766.89 epoch sec; seqNo:2598; depth 49.50 2024-11-06T04:52:46Z- handleDVL() - DVL msg epochMillisec: 1730868766.91 2024-11-06T04:52:46Z- handleDvl() - RDI_Pathfinder msg: 1730868766.91 sec; seqNo:950; #beams:4 2024-11-06T04:52:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.74 , 33.13 , 33.47 , 37.04 2024-11-06T04:52:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , -0.04 2024-11-06T04:52:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868766.93 epoch sec; seqNo:2598; 4061723.59 north; 594806.11 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T04:52:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868766.08 2024-11-06T04:52:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:47Z- waiting for 1730868770.08, time is 1730868766.93 2024-11-06T04:52:47Z- AHRS_M2 msg: 1730868767.28 epoch sec; seqNo:2432 2024-11-06T04:52:47Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.49 psi 2024-11-06T04:52:47Z- handleDepth()- Depth_Keller msg: 1730868767.29 epoch sec; seqNo:2599; depth 49.48 2024-11-06T04:52:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868767.32 epoch sec; seqNo:2599; 4061723.59 north; 594806.11 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T04:52:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868766.08 2024-11-06T04:52:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:47Z- waiting for 1730868770.08, time is 1730868767.32 2024-11-06T04:52:47Z- AHRS_M2 msg: 1730868767.69 epoch sec; seqNo:2433 2024-11-06T04:52:47Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.49 psi 2024-11-06T04:52:47Z- handleDepth()- Depth_Keller msg: 1730868767.69 epoch sec; seqNo:2600; depth 49.48 2024-11-06T04:52:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868767.70 epoch sec; seqNo:2600; 4061723.59 north; 594806.11 east; 36.70 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T04:52:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868766.08 2024-11-06T04:52:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:47Z- waiting for 1730868770.08, time is 1730868767.70 2024-11-06T04:52:48Z- AHRS_M2 msg: 1730868768.09 epoch sec; seqNo:2434 2024-11-06T04:52:48Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.49 psi 2024-11-06T04:52:48Z- handleDepth()- Depth_Keller msg: 1730868768.11 epoch sec; seqNo:2601; depth 49.49 2024-11-06T04:52:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868768.12 epoch sec; seqNo:2601; 4061723.59 north; 594806.11 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:52:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868766.08 2024-11-06T04:52:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:48Z- waiting for 1730868770.08, time is 1730868768.12 2024-11-06T04:52:48Z- AHRS_M2 msg: 1730868768.49 epoch sec; seqNo:2435 2024-11-06T04:52:48Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.49 psi 2024-11-06T04:52:48Z- handleDepth()- Depth_Keller msg: 1730868768.50 epoch sec; seqNo:2602; depth 49.50 2024-11-06T04:52:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868768.51 epoch sec; seqNo:2602; 4061723.60 north; 594807.33 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T04:52:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868766.08 2024-11-06T04:52:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:48Z- waiting for 1730868770.08, time is 1730868768.51 2024-11-06T04:52:48Z- AHRS_M2 msg: 1730868768.90 epoch sec; seqNo:2436 2024-11-06T04:52:48Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.49 psi 2024-11-06T04:52:48Z- handleDepth()- Depth_Keller msg: 1730868768.91 epoch sec; seqNo:2603; depth 49.49 2024-11-06T04:52:48Z- handleDVL() - DVL msg epochMillisec: 1730868768.93 2024-11-06T04:52:48Z- handleDvl() - RDI_Pathfinder msg: 1730868768.93 sec; seqNo:951; #beams:4 2024-11-06T04:52:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.24 , 32.96 , 33.47 , 37.38 2024-11-06T04:52:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , 0.01 2024-11-06T04:52:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868768.95 epoch sec; seqNo:2603; 4061723.60 north; 594807.33 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:52:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868766.08 2024-11-06T04:52:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:49Z- waiting for 1730868770.08, time is 1730868768.95 2024-11-06T04:52:49Z- AHRS_M2 msg: 1730868769.31 epoch sec; seqNo:2437 2024-11-06T04:52:49Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.49 psi 2024-11-06T04:52:49Z- handleDepth()- Depth_Keller msg: 1730868769.32 epoch sec; seqNo:2604; depth 49.49 2024-11-06T04:52:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868769.33 epoch sec; seqNo:2604; 4061723.60 north; 594807.33 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:52:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868766.08 2024-11-06T04:52:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:49Z- waiting for 1730868770.08, time is 1730868769.33 2024-11-06T04:52:49Z- AHRS_M2 msg: 1730868769.71 epoch sec; seqNo:2438 2024-11-06T04:52:49Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.48 psi 2024-11-06T04:52:49Z- handleDepth()- Depth_Keller msg: 1730868769.72 epoch sec; seqNo:2605; depth 49.49 2024-11-06T04:52:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868769.73 epoch sec; seqNo:2605; 4061723.60 north; 594807.33 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:52:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868766.08 2024-11-06T04:52:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:49Z- waiting for 1730868770.08, time is 1730868769.73 2024-11-06T04:52:50Z- AHRS_M2 msg: 1730868770.11 epoch sec; seqNo:2439 2024-11-06T04:52:50Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.48 psi 2024-11-06T04:52:50Z- handleDepth()- Depth_Keller msg: 1730868770.13 epoch sec; seqNo:2606; depth 49.51 2024-11-06T04:52:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868770.13 epoch sec; seqNo:2606; 4061723.62 north; 594808.55 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T04:52:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868766.08 2024-11-06T04:52:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:50Z- waiting for 1730868770.08, time is 1730868770.13 2024-11-06T04:52:50Z- LcmTrn::updateTrn() >>>> pose time 1730868770.13, meas time 1730868768.93 <<<< 2024-11-06T04:52:50Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868770.13 2024-11-06T04:52:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868768.93 sec, ping # 0; waiting for INS data more recent than 1730868766.08... 2024-11-06T04:52:50Z- TerrainNav::motionUpdate() - currEstimate = (4061724, 594808.55, 49.51) 2024-11-06T04:52:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:52:50Z- TerrainNav:: SubcloudNIS is 0.70, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:52:50Z- TNavPF::Projecting Measurements 2024-11-06T04:52:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:52:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:52:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:52:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:52:50Z- TNavFilter::4 good beams. 2024-11-06T04:52:50Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:52:50Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:52:50Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:52:50Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:52:50Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:52:50Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:52:50Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:52:50Z- TerrainMapOctree: (vehN, vehE ) = (4061723.61, 594807.46). 2024-11-06T04:52:50Z- TerrainMapOctree: Min Distance = 63.24. 2024-11-06T04:52:50Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:52:50Z- Forcing Subcloud Comparison 2024-11-06T04:52:50Z- Weighting particles with subcloud comparison 2024-11-06T04:52:50Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:50Z- meanExpectedMeasDiff: 1.506042 delta_rms_squared: 2.631596 alpha: 0.992457 2024-11-06T04:52:50Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:50Z- meanExpectedMeasDiff: -3.621136 delta_rms_squared: 1.478411 alpha: 0.986653 2024-11-06T04:52:50Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:50Z- meanExpectedMeasDiff: -6.823423 delta_rms_squared: 1.790620 alpha: 0.988954 2024-11-06T04:52:50Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:52:50Z- meanExpectedMeasDiff: 3.834883 delta_rms_squared: 2.926179 alpha: 0.993212 2024-11-06T04:52:50Z- Subcloud NIS: 0.748809 2024-11-06T04:52:50Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:52:50Z- TNavPF:: sumWeights = 0.000026 2024-11-06T04:52:50Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:52:50Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T04:52:50Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:52:50Z- TerrainNav::Resampling particle filter... 2024-11-06T04:52:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868768.93 sec. 2024-11-06T04:52:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:52:50Z- variances([0], [2], and psi_berg) = 1.118903 0.634945 0.000000 2024-11-06T04:52:50Z- publishEstimatesDecoder() - at 1730868770.13 sec 2024-11-06T04:52:50Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:52:50Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061667.72 594812.42 56.53 2024-11-06T04:52:50Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061668.48 594812.43 56.30 lat/lon: 36.70 -121.94 2024-11-06T04:52:50Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.12 0.63 0.70 0.00 2024-11-06T04:52:50Z- AHRS_M2 msg: 1730868770.52 epoch sec; seqNo:2440 2024-11-06T04:52:50Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.48 psi 2024-11-06T04:52:50Z- handleDepth()- Depth_Keller msg: 1730868770.54 epoch sec; seqNo:2607; depth 49.50 2024-11-06T04:52:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868770.56 epoch sec; seqNo:2607; 4061724.00 north; 594808.54 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T04:52:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868770.13 2024-11-06T04:52:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:50Z- waiting for 1730868774.13, time is 1730868770.56 2024-11-06T04:52:50Z- AHRS_M2 msg: 1730868770.94 epoch sec; seqNo:2441 2024-11-06T04:52:50Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.48 psi 2024-11-06T04:52:50Z- handleDepth()- Depth_Keller msg: 1730868770.97 epoch sec; seqNo:2608; depth 49.52 2024-11-06T04:52:51Z- handleDVL() - DVL msg epochMillisec: 1730868771.00 2024-11-06T04:52:51Z- handleDvl() - RDI_Pathfinder msg: 1730868771.00 sec; seqNo:952; #beams:4 2024-11-06T04:52:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.40 , 32.96 , 33.81 , 37.21 2024-11-06T04:52:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.03 2024-11-06T04:52:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868771.01 epoch sec; seqNo:2608; 4061724.00 north; 594808.54 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:52:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868770.13 2024-11-06T04:52:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:51Z- waiting for 1730868774.13, time is 1730868771.01 2024-11-06T04:52:51Z- AHRS_M2 msg: 1730868771.32 epoch sec; seqNo:2442 2024-11-06T04:52:51Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.48 psi 2024-11-06T04:52:51Z- handleDepth()- Depth_Keller msg: 1730868771.33 epoch sec; seqNo:2609; depth 49.51 2024-11-06T04:52:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868771.34 epoch sec; seqNo:2609; 4061724.01 north; 594809.76 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T04:52:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868770.13 2024-11-06T04:52:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:51Z- waiting for 1730868774.13, time is 1730868771.34 2024-11-06T04:52:51Z- AHRS_M2 msg: 1730868771.74 epoch sec; seqNo:2443 2024-11-06T04:52:51Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:52:51Z- handleDepth()- Depth_Keller msg: 1730868771.74 epoch sec; seqNo:2610; depth 49.52 2024-11-06T04:52:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868771.75 epoch sec; seqNo:2610; 4061724.01 north; 594809.76 east; 36.70 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T04:52:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868770.13 2024-11-06T04:52:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:51Z- waiting for 1730868774.13, time is 1730868771.75 2024-11-06T04:52:52Z- AHRS_M2 msg: 1730868772.13 epoch sec; seqNo:2444 2024-11-06T04:52:52Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:52:52Z- handleDepth()- Depth_Keller msg: 1730868772.15 epoch sec; seqNo:2611; depth 49.53 2024-11-06T04:52:52Z- handleDVL() - DVL msg epochMillisec: 1730868772.15 2024-11-06T04:52:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:52Z- handleDvl() - RDI_Pathfinder msg: 1730868772.15 sec; seqNo:953; #beams:0 2024-11-06T04:52:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.03 , 0.03 2024-11-06T04:52:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868772.16 epoch sec; seqNo:2611; 4061724.01 north; 594809.76 east; 36.70 lat; -121.94 lon; 49.53 depth; utmZone: 10 2024-11-06T04:52:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868770.13 2024-11-06T04:52:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:52Z- waiting for 1730868774.13, time is 1730868772.16 2024-11-06T04:52:52Z- AHRS_M2 msg: 1730868772.54 epoch sec; seqNo:2445 2024-11-06T04:52:52Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:52:52Z- handleDepth()- Depth_Keller msg: 1730868772.54 epoch sec; seqNo:2612; depth 49.55 2024-11-06T04:52:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868772.56 epoch sec; seqNo:2612; 4061724.01 north; 594809.76 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T04:52:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868770.13 2024-11-06T04:52:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:52Z- waiting for 1730868774.13, time is 1730868772.56 2024-11-06T04:52:52Z- AHRS_M2 msg: 1730868772.94 epoch sec; seqNo:2446 2024-11-06T04:52:52Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:52:52Z- handleDepth()- Depth_Keller msg: 1730868772.95 epoch sec; seqNo:2613; depth 49.54 2024-11-06T04:52:52Z- handleDVL() - DVL msg epochMillisec: 1730868772.97 2024-11-06T04:52:52Z- handleDvl() - RDI_Pathfinder msg: 1730868772.97 sec; seqNo:954; #beams:4 2024-11-06T04:52:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.06 , 33.13 , 33.47 , 37.38 2024-11-06T04:52:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.02 2024-11-06T04:52:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868772.99 epoch sec; seqNo:2613; 4061724.02 north; 594810.98 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T04:52:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868770.13 2024-11-06T04:52:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:53Z- waiting for 1730868774.13, time is 1730868772.99 2024-11-06T04:52:53Z- AHRS_M2 msg: 1730868773.34 epoch sec; seqNo:2447 2024-11-06T04:52:53Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:52:53Z- handleDepth()- Depth_Keller msg: 1730868773.35 epoch sec; seqNo:2614; depth 49.54 2024-11-06T04:52:53Z- handleDVL() - DVL msg epochMillisec: 1730868773.35 2024-11-06T04:52:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:53Z- handleDvl() - RDI_Pathfinder msg: 1730868773.35 sec; seqNo:955; #beams:0 2024-11-06T04:52:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.02 2024-11-06T04:52:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868773.37 epoch sec; seqNo:2614; 4061724.02 north; 594810.98 east; 36.70 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T04:52:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868770.13 2024-11-06T04:52:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:53Z- waiting for 1730868774.13, time is 1730868773.37 2024-11-06T04:52:53Z- AHRS_M2 msg: 1730868773.75 epoch sec; seqNo:2448 2024-11-06T04:52:53Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:52:53Z- handleDepth()- Depth_Keller msg: 1730868773.75 epoch sec; seqNo:2615; depth 49.56 2024-11-06T04:52:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868773.78 epoch sec; seqNo:2615; 4061724.02 north; 594810.98 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:52:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868770.13 2024-11-06T04:52:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:53Z- waiting for 1730868774.13, time is 1730868773.78 2024-11-06T04:52:54Z- AHRS_M2 msg: 1730868774.15 epoch sec; seqNo:2449 2024-11-06T04:52:54Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:52:54Z- handleDepth()- Depth_Keller msg: 1730868774.16 epoch sec; seqNo:2616; depth 49.56 2024-11-06T04:52:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868774.17 epoch sec; seqNo:2616; 4061724.42 north; 594812.20 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:52:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868770.13 2024-11-06T04:52:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:54Z- waiting for 1730868774.13, time is 1730868774.17 2024-11-06T04:52:54Z- LcmTrn::updateTrn() >>>> pose time 1730868774.17, meas time 1730868773.35 <<<< 2024-11-06T04:52:54Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868774.17 2024-11-06T04:52:54Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:52:54Z- TerrainNav::motionUpdate() - currEstimate = (4061724, 594812.20, 49.56) 2024-11-06T04:52:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:52:54Z- TerrainNav:: SubcloudNIS is 0.75, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:52:54Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:52:54Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:52:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:52:54Z- variances([0], [2], and psi_berg) = 1.121948 0.637475 0.000000 2024-11-06T04:52:54Z- publishEstimatesDecoder() - at 1730868774.17 sec 2024-11-06T04:52:54Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:52:54Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061668.52 594816.13 56.35 2024-11-06T04:52:54Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061669.28 594816.09 56.35 lat/lon: 36.70 -121.94 2024-11-06T04:52:54Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.12 0.64 0.94 0.00 2024-11-06T04:52:54Z- AHRS_M2 msg: 1730868774.56 epoch sec; seqNo:2450 2024-11-06T04:52:54Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:52:54Z- handleDepth()- Depth_Keller msg: 1730868774.57 epoch sec; seqNo:2617; depth 49.58 2024-11-06T04:52:54Z- handleDVL() - DVL msg epochMillisec: 1730868774.57 2024-11-06T04:52:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:54Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:54Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:54Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:54Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:54Z- handleDvl() - RDI_Pathfinder msg: 1730868774.57 sec; seqNo:956; #beams:0 2024-11-06T04:52:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.02 2024-11-06T04:52:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868774.58 epoch sec; seqNo:2617; 4061724.42 north; 594812.20 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:52:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868774.17 2024-11-06T04:52:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:54Z- waiting for 1730868778.17, time is 1730868774.58 2024-11-06T04:52:54Z- AHRS_M2 msg: 1730868774.97 epoch sec; seqNo:2451 2024-11-06T04:52:54Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:52:54Z- handleDepth()- Depth_Keller msg: 1730868774.98 epoch sec; seqNo:2618; depth 49.59 2024-11-06T04:52:55Z- handleDVL() - DVL msg epochMillisec: 1730868775.00 2024-11-06T04:52:55Z- handleDvl() - RDI_Pathfinder msg: 1730868775.00 sec; seqNo:957; #beams:4 2024-11-06T04:52:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.72 , 32.62 , 33.81 , 36.87 2024-11-06T04:52:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.03 , -0.01 2024-11-06T04:52:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868775.01 epoch sec; seqNo:2618; 4061724.42 north; 594812.20 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:52:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868774.17 2024-11-06T04:52:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:55Z- waiting for 1730868778.17, time is 1730868775.01 2024-11-06T04:52:55Z- AHRS_M2 msg: 1730868775.36 epoch sec; seqNo:2452 2024-11-06T04:52:55Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:52:55Z- handleDepth()- Depth_Keller msg: 1730868775.37 epoch sec; seqNo:2619; depth 49.59 2024-11-06T04:52:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868775.39 epoch sec; seqNo:2619; 4061724.42 north; 594812.20 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:52:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868774.17 2024-11-06T04:52:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:55Z- waiting for 1730868778.17, time is 1730868775.39 2024-11-06T04:52:55Z- AHRS_M2 msg: 1730868775.78 epoch sec; seqNo:2453 2024-11-06T04:52:55Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T04:52:55Z- handleDepth()- Depth_Keller msg: 1730868775.80 epoch sec; seqNo:2620; depth 49.61 2024-11-06T04:52:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868775.83 epoch sec; seqNo:2620; 4061724.43 north; 594813.42 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:52:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868774.17 2024-11-06T04:52:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:55Z- waiting for 1730868778.17, time is 1730868775.83 2024-11-06T04:52:56Z- AHRS_M2 msg: 1730868776.17 epoch sec; seqNo:2454 2024-11-06T04:52:56Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:52:56Z- handleDepth()- Depth_Keller msg: 1730868776.19 epoch sec; seqNo:2621; depth 49.60 2024-11-06T04:52:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868776.20 epoch sec; seqNo:2621; 4061724.43 north; 594813.42 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:52:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868774.17 2024-11-06T04:52:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:56Z- waiting for 1730868778.17, time is 1730868776.20 2024-11-06T04:52:56Z- AHRS_M2 msg: 1730868776.58 epoch sec; seqNo:2455 2024-11-06T04:52:56Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:52:56Z- handleDepth()- Depth_Keller msg: 1730868776.59 epoch sec; seqNo:2622; depth 49.60 2024-11-06T04:52:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868776.60 epoch sec; seqNo:2622; 4061724.43 north; 594813.42 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:52:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868774.17 2024-11-06T04:52:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:56Z- waiting for 1730868778.17, time is 1730868776.60 2024-11-06T04:52:56Z- AHRS_M2 msg: 1730868776.98 epoch sec; seqNo:2456 2024-11-06T04:52:56Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:52:56Z- handleDepth()- Depth_Keller msg: 1730868776.99 epoch sec; seqNo:2623; depth 49.61 2024-11-06T04:52:57Z- handleDVL() - DVL msg epochMillisec: 1730868777.02 2024-11-06T04:52:57Z- handleDvl() - RDI_Pathfinder msg: 1730868777.02 sec; seqNo:958; #beams:4 2024-11-06T04:52:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.04 , 30.58 , 33.30 , 37.04 2024-11-06T04:52:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.00 2024-11-06T04:52:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868777.03 epoch sec; seqNo:2623; 4061724.44 north; 594814.64 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:52:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868774.17 2024-11-06T04:52:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:57Z- waiting for 1730868778.17, time is 1730868777.03 2024-11-06T04:52:57Z- AHRS_M2 msg: 1730868777.39 epoch sec; seqNo:2457 2024-11-06T04:52:57Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:52:57Z- handleDepth()- Depth_Keller msg: 1730868777.39 epoch sec; seqNo:2624; depth 49.61 2024-11-06T04:52:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868777.40 epoch sec; seqNo:2624; 4061724.44 north; 594814.64 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:52:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868774.17 2024-11-06T04:52:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:57Z- waiting for 1730868778.17, time is 1730868777.40 2024-11-06T04:52:57Z- AHRS_M2 msg: 1730868777.80 epoch sec; seqNo:2458 2024-11-06T04:52:57Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:52:57Z- handleDepth()- Depth_Keller msg: 1730868777.80 epoch sec; seqNo:2625; depth 49.61 2024-11-06T04:52:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868777.82 epoch sec; seqNo:2625; 4061724.82 north; 594814.64 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:52:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868774.17 2024-11-06T04:52:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:57Z- waiting for 1730868778.17, time is 1730868777.82 2024-11-06T04:52:58Z- AHRS_M2 msg: 1730868778.19 epoch sec; seqNo:2459 2024-11-06T04:52:58Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T04:52:58Z- handleDepth()- Depth_Keller msg: 1730868778.20 epoch sec; seqNo:2626; depth 49.61 2024-11-06T04:52:58Z- handleDVL() - DVL msg epochMillisec: 1730868778.20 2024-11-06T04:52:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:58Z- handleDvl() - RDI_Pathfinder msg: 1730868778.20 sec; seqNo:959; #beams:0 2024-11-06T04:52:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.00 2024-11-06T04:52:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868778.21 epoch sec; seqNo:2626; 4061724.82 north; 594814.64 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:52:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868774.17 2024-11-06T04:52:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:58Z- waiting for 1730868778.17, time is 1730868778.21 2024-11-06T04:52:58Z- LcmTrn::updateTrn() >>>> pose time 1730868778.21, meas time 1730868778.20 <<<< 2024-11-06T04:52:58Z- LcmTrn::updateTrn() equating pose 1730868778.20 and meas times 1730868778.20 2024-11-06T04:52:58Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868778.20 2024-11-06T04:52:58Z- TerrainNav::motionUpdate() - currEstimate = (4061725, 594814.64, 49.61) 2024-11-06T04:52:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:52:58Z- TerrainNav:: SubcloudNIS is 0.75, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:52:58Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:52:58Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:52:58Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:52:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:52:58Z- variances([0], [2], and psi_berg) = 1.122713 0.638218 0.000000 2024-11-06T04:52:58Z- publishEstimatesDecoder() - at 1730868778.20 sec 2024-11-06T04:52:58Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:52:58Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061668.92 594818.56 55.89 2024-11-06T04:52:58Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061669.69 594818.52 56.41 lat/lon: 36.70 -121.94 2024-11-06T04:52:58Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.12 0.64 1.19 0.00 2024-11-06T04:52:58Z- AHRS_M2 msg: 1730868778.60 epoch sec; seqNo:2460 2024-11-06T04:52:58Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T04:52:58Z- handleDepth()- Depth_Keller msg: 1730868778.61 epoch sec; seqNo:2627; depth 49.63 2024-11-06T04:52:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868778.62 epoch sec; seqNo:2627; 4061724.84 north; 594815.86 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:52:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868778.20 2024-11-06T04:52:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:58Z- waiting for 1730868782.20, time is 1730868778.62 2024-11-06T04:52:59Z- AHRS_M2 msg: 1730868779.00 epoch sec; seqNo:2461 2024-11-06T04:52:59Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T04:52:59Z- handleDepth()- Depth_Keller msg: 1730868779.01 epoch sec; seqNo:2628; depth 49.63 2024-11-06T04:52:59Z- handleDVL() - DVL msg epochMillisec: 1730868779.03 2024-11-06T04:52:59Z- handleDvl() - RDI_Pathfinder msg: 1730868779.03 sec; seqNo:960; #beams:4 2024-11-06T04:52:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.04 , 31.09 , 33.64 , 36.87 2024-11-06T04:52:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.00 , 0.02 2024-11-06T04:52:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868779.04 epoch sec; seqNo:2628; 4061724.84 north; 594815.86 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:52:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868778.20 2024-11-06T04:52:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:59Z- waiting for 1730868782.20, time is 1730868779.04 2024-11-06T04:52:59Z- AHRS_M2 msg: 1730868779.40 epoch sec; seqNo:2462 2024-11-06T04:52:59Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T04:52:59Z- handleDepth()- Depth_Keller msg: 1730868779.41 epoch sec; seqNo:2629; depth 49.63 2024-11-06T04:52:59Z- handleDVL() - DVL msg epochMillisec: 1730868779.41 2024-11-06T04:52:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:52:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:52:59Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:52:59Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:52:59Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:52:59Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:52:59Z- handleDvl() - RDI_Pathfinder msg: 1730868779.41 sec; seqNo:961; #beams:0 2024-11-06T04:52:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:52:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.00 , 0.02 2024-11-06T04:52:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868779.43 epoch sec; seqNo:2629; 4061724.84 north; 594815.86 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:52:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:52:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868778.20 2024-11-06T04:52:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:59Z- waiting for 1730868782.20, time is 1730868779.43 2024-11-06T04:52:59Z- AHRS_M2 msg: 1730868779.81 epoch sec; seqNo:2463 2024-11-06T04:52:59Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T04:52:59Z- handleDepth()- Depth_Keller msg: 1730868779.82 epoch sec; seqNo:2630; depth 49.63 2024-11-06T04:52:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868779.83 epoch sec; seqNo:2630; 4061724.85 north; 594817.08 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:52:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:52:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868778.20 2024-11-06T04:52:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:52:59Z- waiting for 1730868782.20, time is 1730868779.83 2024-11-06T04:53:00Z- AHRS_M2 msg: 1730868780.22 epoch sec; seqNo:2464 2024-11-06T04:53:00Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T04:53:00Z- handleDepth()- Depth_Keller msg: 1730868780.22 epoch sec; seqNo:2631; depth 49.63 2024-11-06T04:53:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868780.23 epoch sec; seqNo:2631; 4061724.85 north; 594817.08 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:53:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868778.20 2024-11-06T04:53:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:00Z- waiting for 1730868782.20, time is 1730868780.23 2024-11-06T04:53:00Z- AHRS_M2 msg: 1730868780.62 epoch sec; seqNo:2465 2024-11-06T04:53:00Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T04:53:00Z- handleDepth()- Depth_Keller msg: 1730868780.63 epoch sec; seqNo:2632; depth 49.62 2024-11-06T04:53:00Z- handleDVL() - DVL msg epochMillisec: 1730868780.63 2024-11-06T04:53:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:00Z- handleDvl() - RDI_Pathfinder msg: 1730868780.63 sec; seqNo:962; #beams:0 2024-11-06T04:53:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.00 , 0.02 2024-11-06T04:53:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868780.64 epoch sec; seqNo:2632; 4061724.85 north; 594817.08 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:53:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868778.20 2024-11-06T04:53:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:00Z- waiting for 1730868782.20, time is 1730868780.64 2024-11-06T04:53:01Z- AHRS_M2 msg: 1730868781.02 epoch sec; seqNo:2466 2024-11-06T04:53:01Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T04:53:01Z- handleDepth()- Depth_Keller msg: 1730868781.03 epoch sec; seqNo:2633; depth 49.63 2024-11-06T04:53:01Z- handleDVL() - DVL msg epochMillisec: 1730868781.05 2024-11-06T04:53:01Z- handleDvl() - RDI_Pathfinder msg: 1730868781.05 sec; seqNo:963; #beams:4 2024-11-06T04:53:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.87 , 31.09 , 33.64 , 36.87 2024-11-06T04:53:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , 0.01 2024-11-06T04:53:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868781.07 epoch sec; seqNo:2633; 4061724.85 north; 594817.08 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:53:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868778.20 2024-11-06T04:53:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:01Z- waiting for 1730868782.20, time is 1730868781.07 2024-11-06T04:53:01Z- AHRS_M2 msg: 1730868781.42 epoch sec; seqNo:2467 2024-11-06T04:53:01Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T04:53:01Z- handleDepth()- Depth_Keller msg: 1730868781.43 epoch sec; seqNo:2634; depth 49.63 2024-11-06T04:53:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868781.45 epoch sec; seqNo:2634; 4061724.86 north; 594818.30 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:53:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868778.20 2024-11-06T04:53:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:01Z- waiting for 1730868782.20, time is 1730868781.45 2024-11-06T04:53:01Z- AHRS_M2 msg: 1730868781.83 epoch sec; seqNo:2468 2024-11-06T04:53:01Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T04:53:01Z- handleDepth()- Depth_Keller msg: 1730868781.84 epoch sec; seqNo:2635; depth 49.64 2024-11-06T04:53:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868781.85 epoch sec; seqNo:2635; 4061724.86 north; 594818.30 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:53:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868778.20 2024-11-06T04:53:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:01Z- waiting for 1730868782.20, time is 1730868781.85 2024-11-06T04:53:02Z- AHRS_M2 msg: 1730868782.23 epoch sec; seqNo:2469 2024-11-06T04:53:02Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T04:53:02Z- handleDepth()- Depth_Keller msg: 1730868782.24 epoch sec; seqNo:2636; depth 49.63 2024-11-06T04:53:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868782.25 epoch sec; seqNo:2636; 4061725.24 north; 594818.29 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:53:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868778.20 2024-11-06T04:53:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:02Z- waiting for 1730868782.20, time is 1730868782.25 2024-11-06T04:53:02Z- LcmTrn::updateTrn() >>>> pose time 1730868782.25, meas time 1730868781.05 <<<< 2024-11-06T04:53:02Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868782.25 2024-11-06T04:53:02Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868781.05 sec, ping # 0; waiting for INS data more recent than 1730868778.20... 2024-11-06T04:53:02Z- TerrainNav::motionUpdate() - currEstimate = (4061725, 594818.29, 49.63) 2024-11-06T04:53:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:53:02Z- TerrainNav:: SubcloudNIS is 0.75, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:53:02Z- TNavPF::Projecting Measurements 2024-11-06T04:53:02Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:53:02Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:53:02Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:53:02Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:53:02Z- TNavFilter::4 good beams. 2024-11-06T04:53:02Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:53:02Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:53:02Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:53:02Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:53:02Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:53:02Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:53:02Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:53:02Z- TerrainMapOctree: (vehN, vehE ) = (4061725.12, 594817.21). 2024-11-06T04:53:02Z- TerrainMapOctree: Min Distance = 71.67. 2024-11-06T04:53:02Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:53:02Z- Forcing Subcloud Comparison 2024-11-06T04:53:02Z- Weighting particles with subcloud comparison 2024-11-06T04:53:02Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:02Z- meanExpectedMeasDiff: -1.987013 delta_rms_squared: 1.803052 alpha: 0.989029 2024-11-06T04:53:02Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:02Z- meanExpectedMeasDiff: -5.729470 delta_rms_squared: 1.676286 alpha: 0.988210 2024-11-06T04:53:02Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:02Z- meanExpectedMeasDiff: -8.172764 delta_rms_squared: 4.016613 alpha: 0.995045 2024-11-06T04:53:02Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:02Z- meanExpectedMeasDiff: 3.193549 delta_rms_squared: 1.794202 alpha: 0.988976 2024-11-06T04:53:02Z- Subcloud NIS: 0.812937 2024-11-06T04:53:02Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:53:02Z- TNavPF:: sumWeights = 0.000017 2024-11-06T04:53:02Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:53:02Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T04:53:02Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:53:02Z- TerrainNav::Resampling particle filter... 2024-11-06T04:53:02Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868781.05 sec. 2024-11-06T04:53:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:53:02Z- variances([0], [2], and psi_berg) = 1.008746 0.629156 0.000000 2024-11-06T04:53:02Z- publishEstimatesDecoder() - at 1730868782.25 sec 2024-11-06T04:53:02Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:53:02Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061670.68 594822.36 58.46 2024-11-06T04:53:02Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061669.94 594822.27 58.71 lat/lon: 36.70 -121.94 2024-11-06T04:53:02Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.01 0.63 1.05 0.00 2024-11-06T04:53:02Z- AHRS_M2 msg: 1730868782.64 epoch sec; seqNo:2470 2024-11-06T04:53:02Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T04:53:02Z- handleDepth()- Depth_Keller msg: 1730868782.66 epoch sec; seqNo:2637; depth 49.65 2024-11-06T04:53:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868782.68 epoch sec; seqNo:2637; 4061725.26 north; 594819.51 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:53:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868782.25 2024-11-06T04:53:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:02Z- waiting for 1730868786.25, time is 1730868782.68 2024-11-06T04:53:03Z- AHRS_M2 msg: 1730868783.04 epoch sec; seqNo:2471 2024-11-06T04:53:03Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:53:03Z- handleDepth()- Depth_Keller msg: 1730868783.04 epoch sec; seqNo:2638; depth 49.65 2024-11-06T04:53:03Z- handleDVL() - DVL msg epochMillisec: 1730868783.07 2024-11-06T04:53:03Z- handleDvl() - RDI_Pathfinder msg: 1730868783.07 sec; seqNo:964; #beams:4 2024-11-06T04:53:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.54 , 31.09 , 33.64 , 36.36 2024-11-06T04:53:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , -0.00 2024-11-06T04:53:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868783.10 epoch sec; seqNo:2638; 4061725.26 north; 594819.51 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:53:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868782.25 2024-11-06T04:53:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:03Z- waiting for 1730868786.25, time is 1730868783.10 2024-11-06T04:53:03Z- AHRS_M2 msg: 1730868783.44 epoch sec; seqNo:2472 2024-11-06T04:53:03Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:53:03Z- handleDepth()- Depth_Keller msg: 1730868783.45 epoch sec; seqNo:2639; depth 49.66 2024-11-06T04:53:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868783.47 epoch sec; seqNo:2639; 4061725.26 north; 594819.51 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:53:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868782.25 2024-11-06T04:53:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:03Z- waiting for 1730868786.25, time is 1730868783.47 2024-11-06T04:53:03Z- AHRS_M2 msg: 1730868783.85 epoch sec; seqNo:2473 2024-11-06T04:53:03Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:53:03Z- handleDepth()- Depth_Keller msg: 1730868783.86 epoch sec; seqNo:2640; depth 49.65 2024-11-06T04:53:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868783.87 epoch sec; seqNo:2640; 4061725.26 north; 594819.51 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:53:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868782.25 2024-11-06T04:53:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:04Z- waiting for 1730868786.25, time is 1730868783.87 2024-11-06T04:53:04Z- AHRS_M2 msg: 1730868784.25 epoch sec; seqNo:2474 2024-11-06T04:53:04Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:53:04Z- handleDepth()- Depth_Keller msg: 1730868784.25 epoch sec; seqNo:2641; depth 49.64 2024-11-06T04:53:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868784.26 epoch sec; seqNo:2641; 4061725.27 north; 594820.73 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:53:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868782.25 2024-11-06T04:53:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:04Z- waiting for 1730868786.25, time is 1730868784.26 2024-11-06T04:53:04Z- AHRS_M2 msg: 1730868784.65 epoch sec; seqNo:2475 2024-11-06T04:53:04Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:53:04Z- handleDepth()- Depth_Keller msg: 1730868784.67 epoch sec; seqNo:2642; depth 49.67 2024-11-06T04:53:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868784.68 epoch sec; seqNo:2642; 4061725.27 north; 594820.73 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:53:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868782.25 2024-11-06T04:53:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:04Z- waiting for 1730868786.25, time is 1730868784.68 2024-11-06T04:53:05Z- AHRS_M2 msg: 1730868785.06 epoch sec; seqNo:2476 2024-11-06T04:53:05Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:53:05Z- handleDepth()- Depth_Keller msg: 1730868785.07 epoch sec; seqNo:2643; depth 49.67 2024-11-06T04:53:05Z- handleDVL() - DVL msg epochMillisec: 1730868785.12 2024-11-06T04:53:05Z- handleDvl() - RDI_Pathfinder msg: 1730868785.12 sec; seqNo:965; #beams:4 2024-11-06T04:53:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.87 , 31.09 , 33.30 , 34.32 2024-11-06T04:53:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , -0.02 2024-11-06T04:53:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868785.14 epoch sec; seqNo:2643; 4061725.27 north; 594820.73 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:53:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868782.25 2024-11-06T04:53:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:05Z- waiting for 1730868786.25, time is 1730868785.14 2024-11-06T04:53:05Z- AHRS_M2 msg: 1730868785.46 epoch sec; seqNo:2477 2024-11-06T04:53:05Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:53:05Z- handleDepth()- Depth_Keller msg: 1730868785.47 epoch sec; seqNo:2644; depth 49.66 2024-11-06T04:53:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868785.49 epoch sec; seqNo:2644; 4061725.66 north; 594821.95 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:53:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868782.25 2024-11-06T04:53:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:05Z- waiting for 1730868786.25, time is 1730868785.49 2024-11-06T04:53:05Z- AHRS_M2 msg: 1730868785.87 epoch sec; seqNo:2478 2024-11-06T04:53:05Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.48 psi 2024-11-06T04:53:05Z- handleDepth()- Depth_Keller msg: 1730868785.88 epoch sec; seqNo:2645; depth 49.67 2024-11-06T04:53:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868785.89 epoch sec; seqNo:2645; 4061725.66 north; 594821.95 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:53:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868782.25 2024-11-06T04:53:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:06Z- waiting for 1730868786.25, time is 1730868785.89 2024-11-06T04:53:06Z- AHRS_M2 msg: 1730868786.27 epoch sec; seqNo:2479 2024-11-06T04:53:06Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.47 psi 2024-11-06T04:53:06Z- handleDepth()- Depth_Keller msg: 1730868786.28 epoch sec; seqNo:2646; depth 49.67 2024-11-06T04:53:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868786.29 epoch sec; seqNo:2646; 4061725.66 north; 594821.95 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:53:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868782.25 2024-11-06T04:53:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:06Z- waiting for 1730868786.25, time is 1730868786.29 2024-11-06T04:53:06Z- LcmTrn::updateTrn() >>>> pose time 1730868786.29, meas time 1730868785.12 <<<< 2024-11-06T04:53:06Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868786.29 2024-11-06T04:53:06Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868785.12 sec, ping # 0; waiting for INS data more recent than 1730868782.25... 2024-11-06T04:53:06Z- TerrainNav::motionUpdate() - currEstimate = (4061726, 594821.95, 49.67) 2024-11-06T04:53:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:53:06Z- TerrainNav:: SubcloudNIS is 0.81, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:53:06Z- TNavPF::Projecting Measurements 2024-11-06T04:53:06Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:53:06Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:53:06Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:53:06Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:53:06Z- TNavFilter::4 good beams. 2024-11-06T04:53:06Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:53:06Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:53:06Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:53:06Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:53:06Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:53:06Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:53:06Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:53:06Z- TerrainMapOctree: (vehN, vehE ) = (4061725.54, 594820.89). 2024-11-06T04:53:06Z- TerrainMapOctree: Min Distance = 74.99. 2024-11-06T04:53:06Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:53:06Z- Forcing Subcloud Comparison 2024-11-06T04:53:06Z- Weighting particles with subcloud comparison 2024-11-06T04:53:06Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:06Z- meanExpectedMeasDiff: -2.169606 delta_rms_squared: 1.235431 alpha: 0.984069 2024-11-06T04:53:06Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:06Z- meanExpectedMeasDiff: -3.252816 delta_rms_squared: 1.492180 alpha: 0.986774 2024-11-06T04:53:06Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:06Z- meanExpectedMeasDiff: -7.775515 delta_rms_squared: 2.219854 alpha: 0.991071 2024-11-06T04:53:06Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:06Z- meanExpectedMeasDiff: 2.094094 delta_rms_squared: 1.249031 alpha: 0.984240 2024-11-06T04:53:06Z- Subcloud NIS: 0.842968 2024-11-06T04:53:06Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:53:06Z- TNavPF:: sumWeights = 0.000539 2024-11-06T04:53:06Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:53:06Z- TNavPF::First Term of Map Covariance Matrix is 0.00 2024-11-06T04:53:06Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:53:06Z- TerrainNav::Resampling particle filter... 2024-11-06T04:53:06Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868785.12 sec. 2024-11-06T04:53:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:53:06Z- variances([0], [2], and psi_berg) = 0.820726 0.403387 0.000000 2024-11-06T04:53:06Z- publishEstimatesDecoder() - at 1730868786.29 sec 2024-11-06T04:53:06Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:53:06Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061671.12 594826.00 60.87 2024-11-06T04:53:06Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061669.53 594826.37 60.72 lat/lon: 36.70 -121.94 2024-11-06T04:53:06Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.82 0.40 0.59 0.00 2024-11-06T04:53:06Z- AHRS_M2 msg: 1730868786.70 epoch sec; seqNo:2480 2024-11-06T04:53:06Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T04:53:06Z- handleDepth()- Depth_Keller msg: 1730868786.75 epoch sec; seqNo:2647; depth 49.69 2024-11-06T04:53:06Z- handleDVL() - DVL msg epochMillisec: 1730868786.80 2024-11-06T04:53:06Z- handleDvl() - RDI_Pathfinder msg: 1730868786.80 sec; seqNo:966; #beams:4 2024-11-06T04:53:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.85 , 31.04 , 33.33 , 33.03 2024-11-06T04:53:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , -0.00 2024-11-06T04:53:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868786.84 epoch sec; seqNo:2647; 4061725.66 north; 594821.95 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T04:53:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868786.29 2024-11-06T04:53:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:06Z- waiting for 1730868790.29, time is 1730868786.84 2024-11-06T04:53:07Z- AHRS_M2 msg: 1730868787.08 epoch sec; seqNo:2481 2024-11-06T04:53:07Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T04:53:07Z- handleDepth()- Depth_Keller msg: 1730868787.09 epoch sec; seqNo:2648; depth 49.70 2024-11-06T04:53:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868787.11 epoch sec; seqNo:2648; 4061725.67 north; 594823.17 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T04:53:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868786.29 2024-11-06T04:53:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:07Z- waiting for 1730868790.29, time is 1730868787.11 2024-11-06T04:53:07Z- AHRS_M2 msg: 1730868787.48 epoch sec; seqNo:2482 2024-11-06T04:53:07Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T04:53:07Z- handleDepth()- Depth_Keller msg: 1730868787.49 epoch sec; seqNo:2649; depth 49.70 2024-11-06T04:53:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868787.50 epoch sec; seqNo:2649; 4061725.67 north; 594823.17 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T04:53:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868786.29 2024-11-06T04:53:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:07Z- waiting for 1730868790.29, time is 1730868787.50 2024-11-06T04:53:07Z- AHRS_M2 msg: 1730868787.89 epoch sec; seqNo:2483 2024-11-06T04:53:07Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T04:53:07Z- handleDepth()- Depth_Keller msg: 1730868787.89 epoch sec; seqNo:2650; depth 49.70 2024-11-06T04:53:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868787.90 epoch sec; seqNo:2650; 4061725.67 north; 594823.17 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T04:53:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868786.29 2024-11-06T04:53:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:08Z- waiting for 1730868790.29, time is 1730868787.90 2024-11-06T04:53:08Z- AHRS_M2 msg: 1730868788.29 epoch sec; seqNo:2484 2024-11-06T04:53:08Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T04:53:08Z- handleDepth()- Depth_Keller msg: 1730868788.31 epoch sec; seqNo:2651; depth 49.72 2024-11-06T04:53:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868788.31 epoch sec; seqNo:2651; 4061726.05 north; 594823.17 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:53:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868786.29 2024-11-06T04:53:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:08Z- waiting for 1730868790.29, time is 1730868788.31 2024-11-06T04:53:08Z- AHRS_M2 msg: 1730868788.73 epoch sec; seqNo:2485 2024-11-06T04:53:08Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T04:53:08Z- handleDepth()- Depth_Keller msg: 1730868788.75 epoch sec; seqNo:2652; depth 49.71 2024-11-06T04:53:08Z- handleDVL() - DVL msg epochMillisec: 1730868788.79 2024-11-06T04:53:08Z- handleDvl() - RDI_Pathfinder msg: 1730868788.79 sec; seqNo:967; #beams:4 2024-11-06T04:53:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.54 , 31.19 , 33.64 , 33.03 2024-11-06T04:53:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.02 , 0.02 2024-11-06T04:53:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868788.80 epoch sec; seqNo:2652; 4061726.07 north; 594824.39 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T04:53:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868786.29 2024-11-06T04:53:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:08Z- waiting for 1730868790.29, time is 1730868788.80 2024-11-06T04:53:09Z- AHRS_M2 msg: 1730868789.10 epoch sec; seqNo:2486 2024-11-06T04:53:09Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T04:53:09Z- handleDepth()- Depth_Keller msg: 1730868789.11 epoch sec; seqNo:2653; depth 49.72 2024-11-06T04:53:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868789.12 epoch sec; seqNo:2653; 4061726.07 north; 594824.39 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:53:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868786.29 2024-11-06T04:53:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:09Z- waiting for 1730868790.29, time is 1730868789.12 2024-11-06T04:53:09Z- AHRS_M2 msg: 1730868789.50 epoch sec; seqNo:2487 2024-11-06T04:53:09Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T04:53:09Z- handleDepth()- Depth_Keller msg: 1730868789.51 epoch sec; seqNo:2654; depth 49.75 2024-11-06T04:53:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868789.52 epoch sec; seqNo:2654; 4061726.07 north; 594824.39 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:53:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868786.29 2024-11-06T04:53:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:09Z- waiting for 1730868790.29, time is 1730868789.52 2024-11-06T04:53:09Z- AHRS_M2 msg: 1730868789.91 epoch sec; seqNo:2488 2024-11-06T04:53:09Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T04:53:09Z- handleDepth()- Depth_Keller msg: 1730868789.92 epoch sec; seqNo:2655; depth 49.74 2024-11-06T04:53:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868789.93 epoch sec; seqNo:2655; 4061726.08 north; 594825.61 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T04:53:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868786.29 2024-11-06T04:53:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:10Z- waiting for 1730868790.29, time is 1730868789.93 2024-11-06T04:53:10Z- AHRS_M2 msg: 1730868790.31 epoch sec; seqNo:2489 2024-11-06T04:53:10Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.47 psi 2024-11-06T04:53:10Z- handleDepth()- Depth_Keller msg: 1730868790.32 epoch sec; seqNo:2656; depth 49.76 2024-11-06T04:53:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868790.33 epoch sec; seqNo:2656; 4061726.08 north; 594825.61 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T04:53:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868786.29 2024-11-06T04:53:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:10Z- waiting for 1730868790.29, time is 1730868790.33 2024-11-06T04:53:10Z- LcmTrn::updateTrn() >>>> pose time 1730868790.33, meas time 1730868788.79 <<<< 2024-11-06T04:53:10Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868790.33 2024-11-06T04:53:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868788.79 sec, ping # 0; waiting for INS data more recent than 1730868786.29... 2024-11-06T04:53:10Z- TerrainNav::motionUpdate() - currEstimate = (4061726, 594825.61, 49.76) 2024-11-06T04:53:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:53:10Z- TerrainNav:: SubcloudNIS is 0.84, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:53:10Z- TNavPF::Projecting Measurements 2024-11-06T04:53:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:53:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:53:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:53:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:53:10Z- TNavFilter::4 good beams. 2024-11-06T04:53:10Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:53:10Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:53:10Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:53:10Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:53:10Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:53:10Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:53:10Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:53:10Z- TerrainMapOctree: (vehN, vehE ) = (4061725.92, 594824.21). 2024-11-06T04:53:10Z- TerrainMapOctree: Min Distance = 78.02. 2024-11-06T04:53:10Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:53:10Z- Forcing Subcloud Comparison 2024-11-06T04:53:10Z- Weighting particles with subcloud comparison 2024-11-06T04:53:10Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:10Z- meanExpectedMeasDiff: -1.763169 delta_rms_squared: 1.362327 alpha: 0.985532 2024-11-06T04:53:10Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:10Z- meanExpectedMeasDiff: -0.943826 delta_rms_squared: 0.966780 alpha: 0.979732 2024-11-06T04:53:10Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:10Z- meanExpectedMeasDiff: -5.748193 delta_rms_squared: 1.742137 alpha: 0.988650 2024-11-06T04:53:10Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:10Z- meanExpectedMeasDiff: 1.338601 delta_rms_squared: 0.831620 alpha: 0.976515 2024-11-06T04:53:10Z- Subcloud NIS: 0.825583 2024-11-06T04:53:10Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:53:10Z- TNavPF:: sumWeights = 0.013711 2024-11-06T04:53:10Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:53:10Z- TNavPF::First Term of Map Covariance Matrix is 0.01 2024-11-06T04:53:10Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:53:10Z- TerrainNav::Resampling particle filter... 2024-11-06T04:53:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868788.79 sec. 2024-11-06T04:53:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:53:10Z- variances([0], [2], and psi_berg) = 0.693714 0.223783 0.000000 2024-11-06T04:53:10Z- publishEstimatesDecoder() - at 1730868790.33 sec 2024-11-06T04:53:10Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:53:10Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061671.52 594829.73 61.27 2024-11-06T04:53:10Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061669.61 594830.18 61.76 lat/lon: 36.70 -121.94 2024-11-06T04:53:10Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.69 0.22 0.60 0.00 2024-11-06T04:53:10Z- handleDepth()- Depth_Keller msg: 1730868790.79 epoch sec; seqNo:2657; depth 49.76 2024-11-06T04:53:10Z- handleDVL() - DVL msg epochMillisec: 1730868790.85 2024-11-06T04:53:10Z- handleDvl() - RDI_Pathfinder msg: 1730868790.85 sec; seqNo:968; #beams:4 2024-11-06T04:53:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.70 , 30.73 , 33.94 , 33.49 2024-11-06T04:53:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , 0.03 2024-11-06T04:53:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868790.86 epoch sec; seqNo:2657; 4061726.08 north; 594825.61 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T04:53:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868790.33 2024-11-06T04:53:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:10Z- waiting for 1730868794.33, time is 1730868790.86 2024-11-06T04:53:11Z- handleDepth()- Depth_Keller msg: 1730868791.14 epoch sec; seqNo:2658; depth 49.77 2024-11-06T04:53:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868791.15 epoch sec; seqNo:2658; 4061726.08 north; 594825.61 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T04:53:11Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:53:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868790.33 2024-11-06T04:53:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:11Z- waiting for 1730868794.33, time is 1730868791.15 2024-11-06T04:53:11Z- AHRS_M2 msg: 1730868791.53 epoch sec; seqNo:2490 2024-11-06T04:53:11Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.48 psi 2024-11-06T04:53:11Z- handleDepth()- Depth_Keller msg: 1730868791.54 epoch sec; seqNo:2659; depth 49.76 2024-11-06T04:53:11Z- handleDVL() - DVL msg epochMillisec: 1730868791.54 2024-11-06T04:53:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:11Z- handleDvl() - RDI_Pathfinder msg: 1730868791.54 sec; seqNo:969; #beams:0 2024-11-06T04:53:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , 0.03 2024-11-06T04:53:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868791.56 epoch sec; seqNo:2659; 4061726.09 north; 594826.83 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T04:53:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868790.33 2024-11-06T04:53:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:11Z- waiting for 1730868794.33, time is 1730868791.56 2024-11-06T04:53:11Z- AHRS_M2 msg: 1730868791.93 epoch sec; seqNo:2491 2024-11-06T04:53:11Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.48 psi 2024-11-06T04:53:11Z- handleDepth()- Depth_Keller msg: 1730868791.94 epoch sec; seqNo:2660; depth 49.78 2024-11-06T04:53:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868791.95 epoch sec; seqNo:2660; 4061726.09 north; 594826.83 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:53:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868790.33 2024-11-06T04:53:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:12Z- waiting for 1730868794.33, time is 1730868791.95 2024-11-06T04:53:12Z- AHRS_M2 msg: 1730868792.33 epoch sec; seqNo:2492 2024-11-06T04:53:12Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.48 psi 2024-11-06T04:53:12Z- handleDepth()- Depth_Keller msg: 1730868792.34 epoch sec; seqNo:2661; depth 49.79 2024-11-06T04:53:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868792.35 epoch sec; seqNo:2661; 4061726.09 north; 594826.83 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:53:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868790.33 2024-11-06T04:53:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:12Z- waiting for 1730868794.33, time is 1730868792.35 2024-11-06T04:53:12Z- AHRS_M2 msg: 1730868792.76 epoch sec; seqNo:2493 2024-11-06T04:53:12Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.48 psi 2024-11-06T04:53:12Z- handleDepth()- Depth_Keller msg: 1730868792.76 epoch sec; seqNo:2662; depth 49.79 2024-11-06T04:53:12Z- handleDVL() - DVL msg epochMillisec: 1730868792.79 2024-11-06T04:53:12Z- handleDvl() - RDI_Pathfinder msg: 1730868792.79 sec; seqNo:970; #beams:4 2024-11-06T04:53:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.15 , 30.89 , 34.25 , 34.10 2024-11-06T04:53:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , 0.02 2024-11-06T04:53:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868792.80 epoch sec; seqNo:2662; 4061726.49 north; 594828.04 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:53:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868790.33 2024-11-06T04:53:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:12Z- waiting for 1730868794.33, time is 1730868792.80 2024-11-06T04:53:13Z- AHRS_M2 msg: 1730868793.14 epoch sec; seqNo:2494 2024-11-06T04:53:13Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.48 psi 2024-11-06T04:53:13Z- handleDepth()- Depth_Keller msg: 1730868793.14 epoch sec; seqNo:2663; depth 49.79 2024-11-06T04:53:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868793.16 epoch sec; seqNo:2663; 4061726.49 north; 594828.04 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:53:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868790.33 2024-11-06T04:53:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:13Z- waiting for 1730868794.33, time is 1730868793.16 2024-11-06T04:53:13Z- AHRS_M2 msg: 1730868793.55 epoch sec; seqNo:2495 2024-11-06T04:53:13Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.48 psi 2024-11-06T04:53:13Z- handleDepth()- Depth_Keller msg: 1730868793.56 epoch sec; seqNo:2664; depth 49.79 2024-11-06T04:53:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868793.56 epoch sec; seqNo:2664; 4061726.49 north; 594828.04 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:53:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868790.33 2024-11-06T04:53:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:13Z- waiting for 1730868794.33, time is 1730868793.56 2024-11-06T04:53:13Z- AHRS_M2 msg: 1730868793.95 epoch sec; seqNo:2496 2024-11-06T04:53:13Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.48 psi 2024-11-06T04:53:13Z- handleDepth()- Depth_Keller msg: 1730868793.96 epoch sec; seqNo:2665; depth 49.79 2024-11-06T04:53:13Z- handleDVL() - DVL msg epochMillisec: 1730868793.97 2024-11-06T04:53:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:13Z- handleDvl() - RDI_Pathfinder msg: 1730868793.97 sec; seqNo:971; #beams:0 2024-11-06T04:53:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.02 , 0.02 2024-11-06T04:53:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868793.97 epoch sec; seqNo:2665; 4061726.49 north; 594828.04 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:53:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868790.33 2024-11-06T04:53:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:14Z- waiting for 1730868794.33, time is 1730868793.97 2024-11-06T04:53:14Z- AHRS_M2 msg: 1730868794.35 epoch sec; seqNo:2497 2024-11-06T04:53:14Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.48 psi 2024-11-06T04:53:14Z- handleDepth()- Depth_Keller msg: 1730868794.36 epoch sec; seqNo:2666; depth 49.81 2024-11-06T04:53:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868794.37 epoch sec; seqNo:2666; 4061726.50 north; 594829.26 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:53:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868790.33 2024-11-06T04:53:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:14Z- waiting for 1730868794.33, time is 1730868794.37 2024-11-06T04:53:14Z- LcmTrn::updateTrn() >>>> pose time 1730868794.37, meas time 1730868793.97 <<<< 2024-11-06T04:53:14Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868794.37 2024-11-06T04:53:14Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:53:14Z- TerrainNav::motionUpdate() - currEstimate = (4061727, 594829.26, 49.81) 2024-11-06T04:53:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:53:14Z- TerrainNav:: SubcloudNIS is 0.83, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:53:14Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:53:14Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:53:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:53:14Z- variances([0], [2], and psi_berg) = 0.695223 0.225444 0.000000 2024-11-06T04:53:14Z- publishEstimatesDecoder() - at 1730868794.37 sec 2024-11-06T04:53:14Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:53:14Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061671.98 594833.46 61.61 2024-11-06T04:53:14Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061670.03 594833.84 61.82 lat/lon: 36.70 -121.94 2024-11-06T04:53:14Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.23 0.84 0.00 2024-11-06T04:53:14Z- AHRS_M2 msg: 1730868794.76 epoch sec; seqNo:2498 2024-11-06T04:53:14Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.48 psi 2024-11-06T04:53:14Z- handleDepth()- Depth_Keller msg: 1730868794.77 epoch sec; seqNo:2667; depth 49.81 2024-11-06T04:53:14Z- handleDVL() - DVL msg epochMillisec: 1730868794.80 2024-11-06T04:53:14Z- handleDvl() - RDI_Pathfinder msg: 1730868794.80 sec; seqNo:972; #beams:4 2024-11-06T04:53:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.85 , 31.04 , 34.10 , 33.94 2024-11-06T04:53:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.00 2024-11-06T04:53:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868794.82 epoch sec; seqNo:2667; 4061726.50 north; 594829.26 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:53:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868794.37 2024-11-06T04:53:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:14Z- waiting for 1730868798.37, time is 1730868794.82 2024-11-06T04:53:15Z- AHRS_M2 msg: 1730868795.16 epoch sec; seqNo:2499 2024-11-06T04:53:15Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.48 psi 2024-11-06T04:53:15Z- handleDepth()- Depth_Keller msg: 1730868795.17 epoch sec; seqNo:2668; depth 49.82 2024-11-06T04:53:15Z- handleDVL() - DVL msg epochMillisec: 1730868795.17 2024-11-06T04:53:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:15Z- handleDvl() - RDI_Pathfinder msg: 1730868795.17 sec; seqNo:973; #beams:0 2024-11-06T04:53:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.00 2024-11-06T04:53:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868795.18 epoch sec; seqNo:2668; 4061726.50 north; 594829.26 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:53:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868794.37 2024-11-06T04:53:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:15Z- waiting for 1730868798.37, time is 1730868795.18 2024-11-06T04:53:15Z- AHRS_M2 msg: 1730868795.57 epoch sec; seqNo:2500 2024-11-06T04:53:15Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.48 psi 2024-11-06T04:53:15Z- handleDepth()- Depth_Keller msg: 1730868795.58 epoch sec; seqNo:2669; depth 49.82 2024-11-06T04:53:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868795.59 epoch sec; seqNo:2669; 4061726.51 north; 594830.48 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:53:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868794.37 2024-11-06T04:53:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:15Z- waiting for 1730868798.37, time is 1730868795.59 2024-11-06T04:53:15Z- AHRS_M2 msg: 1730868795.97 epoch sec; seqNo:2501 2024-11-06T04:53:15Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.48 psi 2024-11-06T04:53:15Z- handleDepth()- Depth_Keller msg: 1730868795.98 epoch sec; seqNo:2670; depth 49.83 2024-11-06T04:53:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868795.99 epoch sec; seqNo:2670; 4061726.51 north; 594830.48 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:53:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868794.37 2024-11-06T04:53:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:16Z- waiting for 1730868798.37, time is 1730868795.99 2024-11-06T04:53:16Z- AHRS_M2 msg: 1730868796.38 epoch sec; seqNo:2502 2024-11-06T04:53:16Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.48 psi 2024-11-06T04:53:16Z- handleDepth()- Depth_Keller msg: 1730868796.39 epoch sec; seqNo:2671; depth 49.84 2024-11-06T04:53:16Z- handleDVL() - DVL msg epochMillisec: 1730868796.39 2024-11-06T04:53:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:16Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:16Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:16Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:16Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:16Z- handleDvl() - RDI_Pathfinder msg: 1730868796.39 sec; seqNo:974; #beams:0 2024-11-06T04:53:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , -0.00 2024-11-06T04:53:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868796.40 epoch sec; seqNo:2671; 4061726.51 north; 594830.48 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T04:53:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868794.37 2024-11-06T04:53:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:16Z- waiting for 1730868798.37, time is 1730868796.40 2024-11-06T04:53:16Z- AHRS_M2 msg: 1730868796.78 epoch sec; seqNo:2503 2024-11-06T04:53:16Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.48 psi 2024-11-06T04:53:16Z- handleDepth()- Depth_Keller msg: 1730868796.78 epoch sec; seqNo:2672; depth 49.84 2024-11-06T04:53:16Z- handleDVL() - DVL msg epochMillisec: 1730868796.83 2024-11-06T04:53:16Z- handleDvl() - RDI_Pathfinder msg: 1730868796.83 sec; seqNo:975; #beams:4 2024-11-06T04:53:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.54 , 31.50 , 34.10 , 34.40 2024-11-06T04:53:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , -0.01 2024-11-06T04:53:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868796.86 epoch sec; seqNo:2672; 4061726.51 north; 594830.48 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T04:53:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868794.37 2024-11-06T04:53:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:16Z- waiting for 1730868798.37, time is 1730868796.86 2024-11-06T04:53:17Z- AHRS_M2 msg: 1730868797.18 epoch sec; seqNo:2504 2024-11-06T04:53:17Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.48 psi 2024-11-06T04:53:17Z- handleDepth()- Depth_Keller msg: 1730868797.19 epoch sec; seqNo:2673; depth 49.85 2024-11-06T04:53:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868797.20 epoch sec; seqNo:2673; 4061726.91 north; 594831.70 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:53:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868794.37 2024-11-06T04:53:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:17Z- waiting for 1730868798.37, time is 1730868797.20 2024-11-06T04:53:17Z- AHRS_M2 msg: 1730868797.58 epoch sec; seqNo:2505 2024-11-06T04:53:17Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.48 psi 2024-11-06T04:53:17Z- handleDepth()- Depth_Keller msg: 1730868797.59 epoch sec; seqNo:2674; depth 49.85 2024-11-06T04:53:17Z- handleDVL() - DVL msg epochMillisec: 1730868797.59 2024-11-06T04:53:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:17Z- handleDvl() - RDI_Pathfinder msg: 1730868797.59 sec; seqNo:976; #beams:0 2024-11-06T04:53:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , -0.01 2024-11-06T04:53:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868797.61 epoch sec; seqNo:2674; 4061726.91 north; 594831.70 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:53:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868794.37 2024-11-06T04:53:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:17Z- waiting for 1730868798.37, time is 1730868797.61 2024-11-06T04:53:17Z- AHRS_M2 msg: 1730868797.99 epoch sec; seqNo:2506 2024-11-06T04:53:17Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:18Z- handleDepth()- Depth_Keller msg: 1730868797.99 epoch sec; seqNo:2675; depth 49.85 2024-11-06T04:53:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868798.01 epoch sec; seqNo:2675; 4061726.91 north; 594831.70 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:53:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868794.37 2024-11-06T04:53:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:18Z- waiting for 1730868798.37, time is 1730868798.01 2024-11-06T04:53:18Z- AHRS_M2 msg: 1730868798.39 epoch sec; seqNo:2507 2024-11-06T04:53:18Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:18Z- handleDepth()- Depth_Keller msg: 1730868798.40 epoch sec; seqNo:2676; depth 49.86 2024-11-06T04:53:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868798.41 epoch sec; seqNo:2676; 4061726.92 north; 594832.92 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:53:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868794.37 2024-11-06T04:53:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:18Z- waiting for 1730868798.37, time is 1730868798.41 2024-11-06T04:53:18Z- LcmTrn::updateTrn() >>>> pose time 1730868798.41, meas time 1730868797.59 <<<< 2024-11-06T04:53:18Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868798.41 2024-11-06T04:53:18Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:53:18Z- TerrainNav::motionUpdate() - currEstimate = (4061727, 594832.92, 49.86) 2024-11-06T04:53:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:53:18Z- TerrainNav:: SubcloudNIS is 0.83, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:53:18Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:53:18Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:53:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:53:18Z- variances([0], [2], and psi_berg) = 0.697442 0.228500 0.000000 2024-11-06T04:53:18Z- publishEstimatesDecoder() - at 1730868798.41 sec 2024-11-06T04:53:18Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:53:18Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061672.34 594837.07 61.72 2024-11-06T04:53:18Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061670.44 594837.49 61.85 lat/lon: 36.70 -121.94 2024-11-06T04:53:18Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.23 1.10 0.00 2024-11-06T04:53:18Z- AHRS_M2 msg: 1730868798.80 epoch sec; seqNo:2508 2024-11-06T04:53:18Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:18Z- handleDepth()- Depth_Keller msg: 1730868798.81 epoch sec; seqNo:2677; depth 49.86 2024-11-06T04:53:18Z- handleDVL() - DVL msg epochMillisec: 1730868798.85 2024-11-06T04:53:18Z- handleDvl() - RDI_Pathfinder msg: 1730868798.85 sec; seqNo:977; #beams:4 2024-11-06T04:53:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.54 , 31.80 , 34.25 , 33.94 2024-11-06T04:53:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.06 , -0.01 2024-11-06T04:53:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868798.87 epoch sec; seqNo:2677; 4061726.92 north; 594832.92 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:53:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868798.41 2024-11-06T04:53:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:18Z- waiting for 1730868802.41, time is 1730868798.87 2024-11-06T04:53:19Z- AHRS_M2 msg: 1730868799.20 epoch sec; seqNo:2509 2024-11-06T04:53:19Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:19Z- handleDepth()- Depth_Keller msg: 1730868799.21 epoch sec; seqNo:2678; depth 49.87 2024-11-06T04:53:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868799.22 epoch sec; seqNo:2678; 4061726.92 north; 594832.92 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:53:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868798.41 2024-11-06T04:53:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:19Z- waiting for 1730868802.41, time is 1730868799.22 2024-11-06T04:53:19Z- AHRS_M2 msg: 1730868799.61 epoch sec; seqNo:2510 2024-11-06T04:53:19Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:19Z- handleDepth()- Depth_Keller msg: 1730868799.61 epoch sec; seqNo:2679; depth 49.89 2024-11-06T04:53:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868799.63 epoch sec; seqNo:2679; 4061726.92 north; 594832.92 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:53:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868798.41 2024-11-06T04:53:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:19Z- waiting for 1730868802.41, time is 1730868799.63 2024-11-06T04:53:20Z- AHRS_M2 msg: 1730868800.01 epoch sec; seqNo:2511 2024-11-06T04:53:20Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:20Z- handleDepth()- Depth_Keller msg: 1730868800.02 epoch sec; seqNo:2680; depth 49.89 2024-11-06T04:53:20Z- handleDVL() - DVL msg epochMillisec: 1730868800.02 2024-11-06T04:53:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:20Z- handleDvl() - RDI_Pathfinder msg: 1730868800.02 sec; seqNo:978; #beams:0 2024-11-06T04:53:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.06 , -0.01 2024-11-06T04:53:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868800.05 epoch sec; seqNo:2680; 4061726.93 north; 594834.14 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:53:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868798.41 2024-11-06T04:53:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:20Z- waiting for 1730868802.41, time is 1730868800.05 2024-11-06T04:53:20Z- AHRS_M2 msg: 1730868800.41 epoch sec; seqNo:2512 2024-11-06T04:53:20Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:20Z- handleDepth()- Depth_Keller msg: 1730868800.42 epoch sec; seqNo:2681; depth 49.90 2024-11-06T04:53:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868800.43 epoch sec; seqNo:2681; 4061727.31 north; 594834.14 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:53:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868798.41 2024-11-06T04:53:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:20Z- waiting for 1730868802.41, time is 1730868800.43 2024-11-06T04:53:20Z- AHRS_M2 msg: 1730868800.82 epoch sec; seqNo:2513 2024-11-06T04:53:20Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:20Z- handleDepth()- Depth_Keller msg: 1730868800.83 epoch sec; seqNo:2682; depth 49.91 2024-11-06T04:53:20Z- handleDVL() - DVL msg epochMillisec: 1730868800.86 2024-11-06T04:53:20Z- handleDvl() - RDI_Pathfinder msg: 1730868800.86 sec; seqNo:979; #beams:4 2024-11-06T04:53:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.85 , 31.19 , 34.40 , 33.18 2024-11-06T04:53:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , 0.01 2024-11-06T04:53:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868800.90 epoch sec; seqNo:2682; 4061727.31 north; 594834.14 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:53:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868798.41 2024-11-06T04:53:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:20Z- waiting for 1730868802.41, time is 1730868800.90 2024-11-06T04:53:21Z- AHRS_M2 msg: 1730868801.22 epoch sec; seqNo:2514 2024-11-06T04:53:21Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:21Z- handleDepth()- Depth_Keller msg: 1730868801.23 epoch sec; seqNo:2683; depth 49.91 2024-11-06T04:53:21Z- handleDVL() - DVL msg epochMillisec: 1730868801.23 2024-11-06T04:53:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:21Z- handleDvl() - RDI_Pathfinder msg: 1730868801.23 sec; seqNo:980; #beams:0 2024-11-06T04:53:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , 0.01 2024-11-06T04:53:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868801.26 epoch sec; seqNo:2683; 4061727.33 north; 594835.36 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:53:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868798.41 2024-11-06T04:53:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:21Z- waiting for 1730868802.41, time is 1730868801.26 2024-11-06T04:53:21Z- AHRS_M2 msg: 1730868801.62 epoch sec; seqNo:2515 2024-11-06T04:53:21Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:21Z- handleDepth()- Depth_Keller msg: 1730868801.63 epoch sec; seqNo:2684; depth 49.92 2024-11-06T04:53:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868801.64 epoch sec; seqNo:2684; 4061727.33 north; 594835.36 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:53:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868798.41 2024-11-06T04:53:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:21Z- waiting for 1730868802.41, time is 1730868801.64 2024-11-06T04:53:22Z- AHRS_M2 msg: 1730868802.03 epoch sec; seqNo:2516 2024-11-06T04:53:22Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:22Z- handleDepth()- Depth_Keller msg: 1730868802.03 epoch sec; seqNo:2685; depth 49.93 2024-11-06T04:53:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868802.06 epoch sec; seqNo:2685; 4061727.33 north; 594835.36 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T04:53:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868798.41 2024-11-06T04:53:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:22Z- waiting for 1730868802.41, time is 1730868802.06 2024-11-06T04:53:22Z- AHRS_M2 msg: 1730868802.43 epoch sec; seqNo:2517 2024-11-06T04:53:22Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:22Z- handleDepth()- Depth_Keller msg: 1730868802.44 epoch sec; seqNo:2686; depth 49.94 2024-11-06T04:53:22Z- handleDVL() - DVL msg epochMillisec: 1730868802.44 2024-11-06T04:53:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:22Z- handleDvl() - RDI_Pathfinder msg: 1730868802.44 sec; seqNo:981; #beams:0 2024-11-06T04:53:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , 0.01 2024-11-06T04:53:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868802.45 epoch sec; seqNo:2686; 4061727.33 north; 594835.36 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T04:53:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868798.41 2024-11-06T04:53:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:22Z- waiting for 1730868802.41, time is 1730868802.45 2024-11-06T04:53:22Z- LcmTrn::updateTrn() >>>> pose time 1730868802.45, meas time 1730868802.44 <<<< 2024-11-06T04:53:22Z- LcmTrn::updateTrn() equating pose 1730868802.44 and meas times 1730868802.44 2024-11-06T04:53:22Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868802.44 2024-11-06T04:53:22Z- TerrainNav::motionUpdate() - currEstimate = (4061727, 594835.36, 49.94) 2024-11-06T04:53:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:53:22Z- TerrainNav:: SubcloudNIS is 0.83, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:53:22Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:53:22Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:53:22Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:53:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:53:22Z- variances([0], [2], and psi_berg) = 0.699136 0.229460 0.000000 2024-11-06T04:53:22Z- publishEstimatesDecoder() - at 1730868802.44 sec 2024-11-06T04:53:22Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:53:22Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061672.75 594839.49 62.34 2024-11-06T04:53:22Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061670.85 594839.93 61.94 lat/lon: 36.70 -121.94 2024-11-06T04:53:22Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.23 1.34 0.00 2024-11-06T04:53:22Z- AHRS_M2 msg: 1730868802.84 epoch sec; seqNo:2518 2024-11-06T04:53:22Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:22Z- handleDepth()- Depth_Keller msg: 1730868802.86 epoch sec; seqNo:2687; depth 49.95 2024-11-06T04:53:22Z- handleDVL() - DVL msg epochMillisec: 1730868802.88 2024-11-06T04:53:22Z- handleDvl() - RDI_Pathfinder msg: 1730868802.88 sec; seqNo:982; #beams:4 2024-11-06T04:53:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 36.85 , 31.04 , 34.40 , 33.64 2024-11-06T04:53:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.02 2024-11-06T04:53:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868802.89 epoch sec; seqNo:2687; 4061727.34 north; 594836.58 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T04:53:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868802.44 2024-11-06T04:53:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:22Z- waiting for 1730868806.44, time is 1730868802.89 2024-11-06T04:53:23Z- AHRS_M2 msg: 1730868803.24 epoch sec; seqNo:2519 2024-11-06T04:53:23Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:23Z- handleDepth()- Depth_Keller msg: 1730868803.24 epoch sec; seqNo:2688; depth 49.97 2024-11-06T04:53:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868803.26 epoch sec; seqNo:2688; 4061727.34 north; 594836.58 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T04:53:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868802.44 2024-11-06T04:53:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:23Z- waiting for 1730868806.44, time is 1730868803.26 2024-11-06T04:53:23Z- AHRS_M2 msg: 1730868803.65 epoch sec; seqNo:2520 2024-11-06T04:53:23Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:23Z- handleDepth()- Depth_Keller msg: 1730868803.65 epoch sec; seqNo:2689; depth 49.97 2024-11-06T04:53:23Z- handleDVL() - DVL msg epochMillisec: 1730868803.65 2024-11-06T04:53:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:23Z- handleDvl() - RDI_Pathfinder msg: 1730868803.65 sec; seqNo:983; #beams:0 2024-11-06T04:53:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.01 , 0.02 2024-11-06T04:53:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868803.67 epoch sec; seqNo:2689; 4061727.34 north; 594836.58 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T04:53:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868802.44 2024-11-06T04:53:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:23Z- waiting for 1730868806.44, time is 1730868803.67 2024-11-06T04:53:24Z- AHRS_M2 msg: 1730868804.05 epoch sec; seqNo:2521 2024-11-06T04:53:24Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:24Z- handleDepth()- Depth_Keller msg: 1730868804.05 epoch sec; seqNo:2690; depth 49.98 2024-11-06T04:53:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868804.07 epoch sec; seqNo:2690; 4061727.73 north; 594837.79 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T04:53:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868802.44 2024-11-06T04:53:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:24Z- waiting for 1730868806.44, time is 1730868804.07 2024-11-06T04:53:24Z- AHRS_M2 msg: 1730868804.45 epoch sec; seqNo:2522 2024-11-06T04:53:24Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:24Z- handleDepth()- Depth_Keller msg: 1730868804.46 epoch sec; seqNo:2691; depth 49.99 2024-11-06T04:53:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868804.47 epoch sec; seqNo:2691; 4061727.73 north; 594837.79 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T04:53:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868802.44 2024-11-06T04:53:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:24Z- waiting for 1730868806.44, time is 1730868804.47 2024-11-06T04:53:24Z- AHRS_M2 msg: 1730868804.86 epoch sec; seqNo:2523 2024-11-06T04:53:24Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:24Z- handleDepth()- Depth_Keller msg: 1730868804.87 epoch sec; seqNo:2692; depth 49.99 2024-11-06T04:53:24Z- handleDVL() - DVL msg epochMillisec: 1730868804.90 2024-11-06T04:53:24Z- handleDvl() - RDI_Pathfinder msg: 1730868804.90 sec; seqNo:984; #beams:4 2024-11-06T04:53:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.46 , 31.04 , 34.56 , 33.64 2024-11-06T04:53:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , 0.03 2024-11-06T04:53:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868804.91 epoch sec; seqNo:2692; 4061727.73 north; 594837.79 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T04:53:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868802.44 2024-11-06T04:53:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:25Z- waiting for 1730868806.44, time is 1730868804.91 2024-11-06T04:53:25Z- AHRS_M2 msg: 1730868805.26 epoch sec; seqNo:2524 2024-11-06T04:53:25Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:25Z- handleDepth()- Depth_Keller msg: 1730868805.27 epoch sec; seqNo:2693; depth 50.00 2024-11-06T04:53:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868805.28 epoch sec; seqNo:2693; 4061727.73 north; 594837.79 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T04:53:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868802.44 2024-11-06T04:53:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:25Z- waiting for 1730868806.44, time is 1730868805.28 2024-11-06T04:53:25Z- AHRS_M2 msg: 1730868805.66 epoch sec; seqNo:2525 2024-11-06T04:53:25Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:25Z- handleDepth()- Depth_Keller msg: 1730868805.67 epoch sec; seqNo:2694; depth 50.01 2024-11-06T04:53:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868805.69 epoch sec; seqNo:2694; 4061727.74 north; 594839.01 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:53:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868802.44 2024-11-06T04:53:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:25Z- waiting for 1730868806.44, time is 1730868805.69 2024-11-06T04:53:26Z- AHRS_M2 msg: 1730868806.07 epoch sec; seqNo:2526 2024-11-06T04:53:26Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.49 psi 2024-11-06T04:53:26Z- handleDepth()- Depth_Keller msg: 1730868806.08 epoch sec; seqNo:2695; depth 50.02 2024-11-06T04:53:26Z- handleDVL() - DVL msg epochMillisec: 1730868806.08 2024-11-06T04:53:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:26Z- handleDvl() - RDI_Pathfinder msg: 1730868806.08 sec; seqNo:985; #beams:0 2024-11-06T04:53:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , 0.03 2024-11-06T04:53:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868806.10 epoch sec; seqNo:2695; 4061727.74 north; 594839.01 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:53:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868802.44 2024-11-06T04:53:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:26Z- waiting for 1730868806.44, time is 1730868806.10 2024-11-06T04:53:26Z- AHRS_M2 msg: 1730868806.47 epoch sec; seqNo:2527 2024-11-06T04:53:26Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.50 psi 2024-11-06T04:53:26Z- handleDepth()- Depth_Keller msg: 1730868806.47 epoch sec; seqNo:2696; depth 50.02 2024-11-06T04:53:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868806.48 epoch sec; seqNo:2696; 4061727.74 north; 594839.01 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:53:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868802.44 2024-11-06T04:53:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:26Z- waiting for 1730868806.44, time is 1730868806.48 2024-11-06T04:53:26Z- LcmTrn::updateTrn() >>>> pose time 1730868806.48, meas time 1730868806.08 <<<< 2024-11-06T04:53:26Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868806.48 2024-11-06T04:53:26Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:53:26Z- TerrainNav::motionUpdate() - currEstimate = (4061728, 594839.01, 50.02) 2024-11-06T04:53:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:53:26Z- TerrainNav:: SubcloudNIS is 0.83, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:53:26Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:53:26Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:53:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:53:26Z- variances([0], [2], and psi_berg) = 0.702304 0.231579 0.000000 2024-11-06T04:53:26Z- publishEstimatesDecoder() - at 1730868806.48 sec 2024-11-06T04:53:26Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:53:26Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061673.26 594843.16 63.41 2024-11-06T04:53:26Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061671.27 594843.59 62.02 lat/lon: 36.70 -121.94 2024-11-06T04:53:26Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.23 1.57 0.00 2024-11-06T04:53:26Z- AHRS_M2 msg: 1730868806.88 epoch sec; seqNo:2528 2024-11-06T04:53:26Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.50 psi 2024-11-06T04:53:26Z- handleDepth()- Depth_Keller msg: 1730868806.89 epoch sec; seqNo:2697; depth 50.03 2024-11-06T04:53:26Z- handleDVL() - DVL msg epochMillisec: 1730868806.91 2024-11-06T04:53:26Z- handleDvl() - RDI_Pathfinder msg: 1730868806.91 sec; seqNo:986; #beams:4 2024-11-06T04:53:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.00 , 31.19 , 0.00 , 34.10 2024-11-06T04:53:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.01 2024-11-06T04:53:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868806.94 epoch sec; seqNo:2697; 4061727.76 north; 594840.23 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T04:53:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868806.48 2024-11-06T04:53:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:27Z- waiting for 1730868810.48, time is 1730868806.94 2024-11-06T04:53:27Z- AHRS_M2 msg: 1730868807.28 epoch sec; seqNo:2529 2024-11-06T04:53:27Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.50 psi 2024-11-06T04:53:27Z- handleDepth()- Depth_Keller msg: 1730868807.28 epoch sec; seqNo:2698; depth 50.03 2024-11-06T04:53:27Z- handleDVL() - DVL msg epochMillisec: 1730868807.29 2024-11-06T04:53:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:27Z- handleDvl() - RDI_Pathfinder msg: 1730868807.29 sec; seqNo:987; #beams:0 2024-11-06T04:53:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.01 2024-11-06T04:53:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868807.32 epoch sec; seqNo:2698; 4061727.76 north; 594840.23 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T04:53:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868806.48 2024-11-06T04:53:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:27Z- waiting for 1730868810.48, time is 1730868807.32 2024-11-06T04:53:27Z- AHRS_M2 msg: 1730868807.68 epoch sec; seqNo:2530 2024-11-06T04:53:27Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.50 psi 2024-11-06T04:53:27Z- handleDepth()- Depth_Keller msg: 1730868807.69 epoch sec; seqNo:2699; depth 50.03 2024-11-06T04:53:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868807.70 epoch sec; seqNo:2699; 4061727.76 north; 594840.23 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T04:53:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868806.48 2024-11-06T04:53:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:27Z- waiting for 1730868810.48, time is 1730868807.70 2024-11-06T04:53:28Z- AHRS_M2 msg: 1730868808.09 epoch sec; seqNo:2531 2024-11-06T04:53:28Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.50 psi 2024-11-06T04:53:28Z- handleDepth()- Depth_Keller msg: 1730868808.10 epoch sec; seqNo:2700; depth 50.05 2024-11-06T04:53:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868808.11 epoch sec; seqNo:2700; 4061727.76 north; 594840.23 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T04:53:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868806.48 2024-11-06T04:53:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:28Z- waiting for 1730868810.48, time is 1730868808.11 2024-11-06T04:53:28Z- AHRS_M2 msg: 1730868808.49 epoch sec; seqNo:2532 2024-11-06T04:53:28Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.50 psi 2024-11-06T04:53:28Z- handleDepth()- Depth_Keller msg: 1730868808.50 epoch sec; seqNo:2701; depth 50.04 2024-11-06T04:53:28Z- handleDVL() - DVL msg epochMillisec: 1730868808.50 2024-11-06T04:53:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:28Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:28Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:28Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:28Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:28Z- handleDvl() - RDI_Pathfinder msg: 1730868808.50 sec; seqNo:988; #beams:0 2024-11-06T04:53:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.00 , 0.01 2024-11-06T04:53:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868808.51 epoch sec; seqNo:2701; 4061727.77 north; 594841.46 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T04:53:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868806.48 2024-11-06T04:53:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:28Z- waiting for 1730868810.48, time is 1730868808.51 2024-11-06T04:53:28Z- AHRS_M2 msg: 1730868808.90 epoch sec; seqNo:2533 2024-11-06T04:53:28Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T04:53:28Z- handleDepth()- Depth_Keller msg: 1730868808.90 epoch sec; seqNo:2702; depth 50.04 2024-11-06T04:53:28Z- handleDVL() - DVL msg epochMillisec: 1730868808.93 2024-11-06T04:53:28Z- handleDvl() - RDI_Pathfinder msg: 1730868808.93 sec; seqNo:989; #beams:4 2024-11-06T04:53:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.46 , 31.19 , 37.15 , 33.94 2024-11-06T04:53:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , 0.00 2024-11-06T04:53:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868808.94 epoch sec; seqNo:2702; 4061727.77 north; 594841.46 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T04:53:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868806.48 2024-11-06T04:53:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:29Z- waiting for 1730868810.48, time is 1730868808.94 2024-11-06T04:53:29Z- AHRS_M2 msg: 1730868809.30 epoch sec; seqNo:2534 2024-11-06T04:53:29Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T04:53:29Z- handleDepth()- Depth_Keller msg: 1730868809.31 epoch sec; seqNo:2703; depth 50.06 2024-11-06T04:53:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868809.32 epoch sec; seqNo:2703; 4061727.77 north; 594841.46 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T04:53:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868806.48 2024-11-06T04:53:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:29Z- waiting for 1730868810.48, time is 1730868809.32 2024-11-06T04:53:29Z- AHRS_M2 msg: 1730868809.71 epoch sec; seqNo:2535 2024-11-06T04:53:29Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T04:53:29Z- handleDepth()- Depth_Keller msg: 1730868809.71 epoch sec; seqNo:2704; depth 50.05 2024-11-06T04:53:29Z- handleDVL() - DVL msg epochMillisec: 1730868809.72 2024-11-06T04:53:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:29Z- handleDvl() - RDI_Pathfinder msg: 1730868809.72 sec; seqNo:990; #beams:0 2024-11-06T04:53:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , 0.00 2024-11-06T04:53:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868809.72 epoch sec; seqNo:2704; 4061727.79 north; 594842.68 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T04:53:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868806.48 2024-11-06T04:53:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:29Z- waiting for 1730868810.48, time is 1730868809.72 2024-11-06T04:53:30Z- AHRS_M2 msg: 1730868810.11 epoch sec; seqNo:2536 2024-11-06T04:53:30Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T04:53:30Z- handleDepth()- Depth_Keller msg: 1730868810.12 epoch sec; seqNo:2705; depth 50.05 2024-11-06T04:53:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868810.12 epoch sec; seqNo:2705; 4061728.16 north; 594842.67 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T04:53:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868806.48 2024-11-06T04:53:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:30Z- waiting for 1730868810.48, time is 1730868810.12 2024-11-06T04:53:30Z- AHRS_M2 msg: 1730868810.51 epoch sec; seqNo:2537 2024-11-06T04:53:30Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T04:53:30Z- handleDepth()- Depth_Keller msg: 1730868810.52 epoch sec; seqNo:2706; depth 50.06 2024-11-06T04:53:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868810.52 epoch sec; seqNo:2706; 4061728.16 north; 594842.67 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T04:53:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868806.48 2024-11-06T04:53:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:30Z- waiting for 1730868810.48, time is 1730868810.52 2024-11-06T04:53:30Z- LcmTrn::updateTrn() >>>> pose time 1730868810.52, meas time 1730868809.72 <<<< 2024-11-06T04:53:30Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868810.52 2024-11-06T04:53:30Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:53:30Z- TerrainNav::motionUpdate() - currEstimate = (4061728, 594842.67, 50.06) 2024-11-06T04:53:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:53:30Z- TerrainNav:: SubcloudNIS is 0.83, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:53:30Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:53:30Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:53:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:53:30Z- variances([0], [2], and psi_berg) = 0.705476 0.233124 0.000000 2024-11-06T04:53:30Z- publishEstimatesDecoder() - at 1730868810.52 sec 2024-11-06T04:53:30Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:53:30Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061673.67 594846.76 63.30 2024-11-06T04:53:30Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061671.69 594847.25 62.05 lat/lon: 36.70 -121.94 2024-11-06T04:53:30Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.71 0.23 1.83 0.00 2024-11-06T04:53:30Z- AHRS_M2 msg: 1730868810.91 epoch sec; seqNo:2538 2024-11-06T04:53:30Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T04:53:30Z- handleDepth()- Depth_Keller msg: 1730868810.93 epoch sec; seqNo:2707; depth 50.08 2024-11-06T04:53:30Z- handleDVL() - DVL msg epochMillisec: 1730868810.97 2024-11-06T04:53:30Z- handleDvl() - RDI_Pathfinder msg: 1730868810.97 sec; seqNo:991; #beams:4 2024-11-06T04:53:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.72 , 31.09 , 38.90 , 34.15 2024-11-06T04:53:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , -0.01 2024-11-06T04:53:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868811.01 epoch sec; seqNo:2707; 4061728.16 north; 594842.67 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T04:53:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868810.52 2024-11-06T04:53:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:31Z- waiting for 1730868814.52, time is 1730868811.01 2024-11-06T04:53:31Z- AHRS_M2 msg: 1730868811.32 epoch sec; seqNo:2539 2024-11-06T04:53:31Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T04:53:31Z- handleDepth()- Depth_Keller msg: 1730868811.32 epoch sec; seqNo:2708; depth 50.08 2024-11-06T04:53:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868811.34 epoch sec; seqNo:2708; 4061728.18 north; 594843.89 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T04:53:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868810.52 2024-11-06T04:53:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:31Z- waiting for 1730868814.52, time is 1730868811.34 2024-11-06T04:53:31Z- AHRS_M2 msg: 1730868811.73 epoch sec; seqNo:2540 2024-11-06T04:53:31Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T04:53:31Z- handleDepth()- Depth_Keller msg: 1730868811.74 epoch sec; seqNo:2709; depth 50.09 2024-11-06T04:53:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868811.75 epoch sec; seqNo:2709; 4061728.18 north; 594843.89 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T04:53:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868810.52 2024-11-06T04:53:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:31Z- waiting for 1730868814.52, time is 1730868811.75 2024-11-06T04:53:32Z- AHRS_M2 msg: 1730868812.13 epoch sec; seqNo:2541 2024-11-06T04:53:32Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.51 psi 2024-11-06T04:53:32Z- handleDepth()- Depth_Keller msg: 1730868812.13 epoch sec; seqNo:2710; depth 50.09 2024-11-06T04:53:32Z- handleDVL() - DVL msg epochMillisec: 1730868812.15 2024-11-06T04:53:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:32Z- handleDvl() - RDI_Pathfinder msg: 1730868812.15 sec; seqNo:992; #beams:0 2024-11-06T04:53:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , -0.01 2024-11-06T04:53:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868812.17 epoch sec; seqNo:2710; 4061728.18 north; 594843.89 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T04:53:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868810.52 2024-11-06T04:53:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:32Z- waiting for 1730868814.52, time is 1730868812.17 2024-11-06T04:53:32Z- AHRS_M2 msg: 1730868812.53 epoch sec; seqNo:2542 2024-11-06T04:53:32Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.51 psi 2024-11-06T04:53:32Z- handleDepth()- Depth_Keller msg: 1730868812.54 epoch sec; seqNo:2711; depth 50.10 2024-11-06T04:53:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868812.54 epoch sec; seqNo:2711; 4061728.19 north; 594845.11 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T04:53:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868810.52 2024-11-06T04:53:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:32Z- waiting for 1730868814.52, time is 1730868812.54 2024-11-06T04:53:32Z- AHRS_M2 msg: 1730868812.93 epoch sec; seqNo:2543 2024-11-06T04:53:32Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.52 psi 2024-11-06T04:53:32Z- handleDepth()- Depth_Keller msg: 1730868812.95 epoch sec; seqNo:2712; depth 50.12 2024-11-06T04:53:32Z- handleDVL() - DVL msg epochMillisec: 1730868812.97 2024-11-06T04:53:32Z- handleDvl() - RDI_Pathfinder msg: 1730868812.97 sec; seqNo:993; #beams:4 2024-11-06T04:53:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.54 , 30.92 , 38.74 , 33.98 2024-11-06T04:53:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.02 2024-11-06T04:53:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868812.99 epoch sec; seqNo:2712; 4061728.19 north; 594845.11 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T04:53:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868810.52 2024-11-06T04:53:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:33Z- waiting for 1730868814.52, time is 1730868812.99 2024-11-06T04:53:33Z- AHRS_M2 msg: 1730868813.34 epoch sec; seqNo:2544 2024-11-06T04:53:33Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.52 psi 2024-11-06T04:53:33Z- handleDepth()- Depth_Keller msg: 1730868813.34 epoch sec; seqNo:2713; depth 50.12 2024-11-06T04:53:33Z- handleDVL() - DVL msg epochMillisec: 1730868813.35 2024-11-06T04:53:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:33Z- handleDvl() - RDI_Pathfinder msg: 1730868813.35 sec; seqNo:994; #beams:0 2024-11-06T04:53:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.02 2024-11-06T04:53:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868813.36 epoch sec; seqNo:2713; 4061728.19 north; 594845.11 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T04:53:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868810.52 2024-11-06T04:53:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:33Z- waiting for 1730868814.52, time is 1730868813.36 2024-11-06T04:53:33Z- AHRS_M2 msg: 1730868813.74 epoch sec; seqNo:2545 2024-11-06T04:53:33Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.52 psi 2024-11-06T04:53:33Z- handleDepth()- Depth_Keller msg: 1730868813.76 epoch sec; seqNo:2714; depth 50.14 2024-11-06T04:53:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868813.77 epoch sec; seqNo:2714; 4061728.19 north; 594845.11 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:53:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868810.52 2024-11-06T04:53:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:33Z- waiting for 1730868814.52, time is 1730868813.77 2024-11-06T04:53:34Z- AHRS_M2 msg: 1730868814.15 epoch sec; seqNo:2546 2024-11-06T04:53:34Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:53:34Z- handleDepth()- Depth_Keller msg: 1730868814.15 epoch sec; seqNo:2715; depth 50.16 2024-11-06T04:53:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868814.16 epoch sec; seqNo:2715; 4061728.20 north; 594846.33 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T04:53:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868810.52 2024-11-06T04:53:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:34Z- waiting for 1730868814.52, time is 1730868814.16 2024-11-06T04:53:34Z- AHRS_M2 msg: 1730868814.55 epoch sec; seqNo:2547 2024-11-06T04:53:34Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:53:34Z- handleDepth()- Depth_Keller msg: 1730868814.56 epoch sec; seqNo:2716; depth 50.15 2024-11-06T04:53:34Z- handleDVL() - DVL msg epochMillisec: 1730868814.56 2024-11-06T04:53:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:34Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:34Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:34Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:34Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:34Z- handleDvl() - RDI_Pathfinder msg: 1730868814.56 sec; seqNo:995; #beams:0 2024-11-06T04:53:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , -0.02 2024-11-06T04:53:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868814.57 epoch sec; seqNo:2716; 4061728.58 north; 594846.33 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T04:53:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868810.52 2024-11-06T04:53:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:34Z- waiting for 1730868814.52, time is 1730868814.57 2024-11-06T04:53:34Z- LcmTrn::updateTrn() >>>> pose time 1730868814.57, meas time 1730868814.56 <<<< 2024-11-06T04:53:34Z- LcmTrn::updateTrn() equating pose 1730868814.56 and meas times 1730868814.56 2024-11-06T04:53:34Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868814.56 2024-11-06T04:53:34Z- TerrainNav::motionUpdate() - currEstimate = (4061729, 594846.33, 50.15) 2024-11-06T04:53:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:53:34Z- TerrainNav:: SubcloudNIS is 0.83, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:53:34Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:53:34Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:53:34Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:53:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:53:34Z- variances([0], [2], and psi_berg) = 0.706665 0.235465 0.000000 2024-11-06T04:53:34Z- publishEstimatesDecoder() - at 1730868814.56 sec 2024-11-06T04:53:34Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:53:34Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061674.00 594850.45 62.50 2024-11-06T04:53:34Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061672.11 594850.90 62.13 lat/lon: 36.70 -121.94 2024-11-06T04:53:34Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.71 0.24 2.06 0.00 2024-11-06T04:53:34Z- AHRS_M2 msg: 1730868814.95 epoch sec; seqNo:2548 2024-11-06T04:53:34Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:53:34Z- handleDepth()- Depth_Keller msg: 1730868814.97 epoch sec; seqNo:2717; depth 50.17 2024-11-06T04:53:34Z- handleDVL() - DVL msg epochMillisec: 1730868814.99 2024-11-06T04:53:34Z- handleDvl() - RDI_Pathfinder msg: 1730868814.99 sec; seqNo:996; #beams:4 2024-11-06T04:53:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.72 , 31.43 , 39.24 , 33.64 2024-11-06T04:53:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , 0.00 2024-11-06T04:53:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868815.00 epoch sec; seqNo:2717; 4061728.58 north; 594846.33 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T04:53:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868814.56 2024-11-06T04:53:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:35Z- waiting for 1730868818.56, time is 1730868815.00 2024-11-06T04:53:35Z- AHRS_M2 msg: 1730868815.36 epoch sec; seqNo:2549 2024-11-06T04:53:35Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:53:35Z- handleDepth()- Depth_Keller msg: 1730868815.36 epoch sec; seqNo:2718; depth 50.19 2024-11-06T04:53:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868815.38 epoch sec; seqNo:2718; 4061728.60 north; 594847.55 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:53:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868814.56 2024-11-06T04:53:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:35Z- waiting for 1730868818.56, time is 1730868815.38 2024-11-06T04:53:35Z- AHRS_M2 msg: 1730868815.76 epoch sec; seqNo:2550 2024-11-06T04:53:35Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:53:35Z- handleDepth()- Depth_Keller msg: 1730868815.77 epoch sec; seqNo:2719; depth 50.21 2024-11-06T04:53:35Z- handleDVL() - DVL msg epochMillisec: 1730868815.77 2024-11-06T04:53:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:35Z- handleDvl() - RDI_Pathfinder msg: 1730868815.77 sec; seqNo:997; #beams:0 2024-11-06T04:53:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.04 , 0.00 2024-11-06T04:53:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868815.78 epoch sec; seqNo:2719; 4061728.60 north; 594847.55 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T04:53:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868814.56 2024-11-06T04:53:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:35Z- waiting for 1730868818.56, time is 1730868815.78 2024-11-06T04:53:36Z- AHRS_M2 msg: 1730868816.17 epoch sec; seqNo:2551 2024-11-06T04:53:36Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:53:36Z- handleDepth()- Depth_Keller msg: 1730868816.18 epoch sec; seqNo:2720; depth 50.24 2024-11-06T04:53:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868816.19 epoch sec; seqNo:2720; 4061728.60 north; 594847.55 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:53:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868814.56 2024-11-06T04:53:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:36Z- waiting for 1730868818.56, time is 1730868816.19 2024-11-06T04:53:36Z- AHRS_M2 msg: 1730868816.57 epoch sec; seqNo:2552 2024-11-06T04:53:36Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.51 psi 2024-11-06T04:53:36Z- handleDepth()- Depth_Keller msg: 1730868816.58 epoch sec; seqNo:2721; depth 50.24 2024-11-06T04:53:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868816.60 epoch sec; seqNo:2721; 4061728.60 north; 594847.55 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:53:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868814.56 2024-11-06T04:53:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:36Z- waiting for 1730868818.56, time is 1730868816.60 2024-11-06T04:53:36Z- AHRS_M2 msg: 1730868816.97 epoch sec; seqNo:2553 2024-11-06T04:53:36Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.51 psi 2024-11-06T04:53:36Z- handleDepth()- Depth_Keller msg: 1730868816.99 epoch sec; seqNo:2722; depth 50.26 2024-11-06T04:53:37Z- handleDVL() - DVL msg epochMillisec: 1730868817.01 2024-11-06T04:53:37Z- handleDvl() - RDI_Pathfinder msg: 1730868817.01 sec; seqNo:998; #beams:4 2024-11-06T04:53:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.55 , 31.94 , 39.59 , 33.81 2024-11-06T04:53:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.04 2024-11-06T04:53:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868817.03 epoch sec; seqNo:2722; 4061728.61 north; 594848.77 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:53:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868814.56 2024-11-06T04:53:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:37Z- waiting for 1730868818.56, time is 1730868817.03 2024-11-06T04:53:37Z- AHRS_M2 msg: 1730868817.38 epoch sec; seqNo:2554 2024-11-06T04:53:37Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:53:37Z- handleDepth()- Depth_Keller msg: 1730868817.38 epoch sec; seqNo:2723; depth 50.27 2024-11-06T04:53:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868817.40 epoch sec; seqNo:2723; 4061728.61 north; 594848.77 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T04:53:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868814.56 2024-11-06T04:53:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:37Z- waiting for 1730868818.56, time is 1730868817.40 2024-11-06T04:53:37Z- AHRS_M2 msg: 1730868817.79 epoch sec; seqNo:2555 2024-11-06T04:53:37Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:53:37Z- handleDepth()- Depth_Keller msg: 1730868817.81 epoch sec; seqNo:2724; depth 50.29 2024-11-06T04:53:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868817.83 epoch sec; seqNo:2724; 4061728.61 north; 594848.77 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:53:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868814.56 2024-11-06T04:53:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:37Z- waiting for 1730868818.56, time is 1730868817.83 2024-11-06T04:53:38Z- AHRS_M2 msg: 1730868818.19 epoch sec; seqNo:2556 2024-11-06T04:53:38Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:53:38Z- handleDepth()- Depth_Keller msg: 1730868818.20 epoch sec; seqNo:2725; depth 50.29 2024-11-06T04:53:38Z- handleDVL() - DVL msg epochMillisec: 1730868818.20 2024-11-06T04:53:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:38Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:38Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:38Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:38Z- handleDvl() - RDI_Pathfinder msg: 1730868818.20 sec; seqNo:999; #beams:0 2024-11-06T04:53:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.04 2024-11-06T04:53:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868818.21 epoch sec; seqNo:2725; 4061728.61 north; 594848.77 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:53:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868814.56 2024-11-06T04:53:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:38Z- waiting for 1730868818.56, time is 1730868818.21 2024-11-06T04:53:38Z- AHRS_M2 msg: 1730868818.59 epoch sec; seqNo:2557 2024-11-06T04:53:38Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:53:38Z- handleDepth()- Depth_Keller msg: 1730868818.60 epoch sec; seqNo:2726; depth 50.31 2024-11-06T04:53:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868818.61 epoch sec; seqNo:2726; 4061728.62 north; 594849.99 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:53:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868814.56 2024-11-06T04:53:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:38Z- waiting for 1730868818.56, time is 1730868818.61 2024-11-06T04:53:38Z- LcmTrn::updateTrn() >>>> pose time 1730868818.61, meas time 1730868818.20 <<<< 2024-11-06T04:53:38Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868818.61 2024-11-06T04:53:38Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:53:38Z- TerrainNav::motionUpdate() - currEstimate = (4061729, 594849.99, 50.31) 2024-11-06T04:53:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:53:38Z- TerrainNav:: SubcloudNIS is 0.83, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:53:38Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:53:38Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:53:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:53:38Z- variances([0], [2], and psi_berg) = 0.708309 0.237439 0.000000 2024-11-06T04:53:38Z- publishEstimatesDecoder() - at 1730868818.61 sec 2024-11-06T04:53:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:53:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061674.06 594854.07 63.45 2024-11-06T04:53:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061672.15 594854.56 62.29 lat/lon: 36.70 -121.94 2024-11-06T04:53:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.71 0.24 2.31 0.00 2024-11-06T04:53:39Z- AHRS_M2 msg: 1730868818.99 epoch sec; seqNo:2558 2024-11-06T04:53:39Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:53:39Z- handleDepth()- Depth_Keller msg: 1730868819.00 epoch sec; seqNo:2727; depth 50.32 2024-11-06T04:53:39Z- handleDVL() - DVL msg epochMillisec: 1730868819.02 2024-11-06T04:53:39Z- handleDvl() - RDI_Pathfinder msg: 1730868819.02 sec; seqNo:1000; #beams:4 2024-11-06T04:53:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 37.72 , 31.94 , 39.41 , 33.81 2024-11-06T04:53:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , 0.06 2024-11-06T04:53:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868819.05 epoch sec; seqNo:2727; 4061728.62 north; 594849.99 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:53:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868818.61 2024-11-06T04:53:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:39Z- waiting for 1730868822.61, time is 1730868819.05 2024-11-06T04:53:39Z- AHRS_M2 msg: 1730868819.40 epoch sec; seqNo:2559 2024-11-06T04:53:39Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:53:39Z- handleDepth()- Depth_Keller msg: 1730868819.41 epoch sec; seqNo:2728; depth 50.33 2024-11-06T04:53:39Z- handleDVL() - DVL msg epochMillisec: 1730868819.41 2024-11-06T04:53:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:39Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:39Z- handleDvl() - RDI_Pathfinder msg: 1730868819.41 sec; seqNo:1001; #beams:0 2024-11-06T04:53:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , 0.06 2024-11-06T04:53:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868819.42 epoch sec; seqNo:2728; 4061729.00 north; 594849.98 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T04:53:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868818.61 2024-11-06T04:53:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:39Z- waiting for 1730868822.61, time is 1730868819.42 2024-11-06T04:53:39Z- AHRS_M2 msg: 1730868819.80 epoch sec; seqNo:2560 2024-11-06T04:53:39Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:53:39Z- handleDepth()- Depth_Keller msg: 1730868819.82 epoch sec; seqNo:2729; depth 50.34 2024-11-06T04:53:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868819.83 epoch sec; seqNo:2729; 4061729.02 north; 594851.21 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:53:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868818.61 2024-11-06T04:53:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:39Z- waiting for 1730868822.61, time is 1730868819.83 2024-11-06T04:53:40Z- AHRS_M2 msg: 1730868820.21 epoch sec; seqNo:2561 2024-11-06T04:53:40Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:53:40Z- handleDepth()- Depth_Keller msg: 1730868820.22 epoch sec; seqNo:2730; depth 50.35 2024-11-06T04:53:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868820.22 epoch sec; seqNo:2730; 4061729.02 north; 594851.21 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:53:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868818.61 2024-11-06T04:53:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:40Z- waiting for 1730868822.61, time is 1730868820.22 2024-11-06T04:53:40Z- AHRS_M2 msg: 1730868820.61 epoch sec; seqNo:2562 2024-11-06T04:53:40Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:53:40Z- handleDepth()- Depth_Keller msg: 1730868820.62 epoch sec; seqNo:2731; depth 50.34 2024-11-06T04:53:40Z- handleDVL() - DVL msg epochMillisec: 1730868820.62 2024-11-06T04:53:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:40Z- handleDvl() - RDI_Pathfinder msg: 1730868820.62 sec; seqNo:1002; #beams:0 2024-11-06T04:53:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , 0.06 2024-11-06T04:53:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868820.63 epoch sec; seqNo:2731; 4061729.02 north; 594851.21 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:53:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868818.61 2024-11-06T04:53:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:40Z- waiting for 1730868822.61, time is 1730868820.63 2024-11-06T04:53:41Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:53:41Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:53:41Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:53:41Z- AHRS_M2 msg: 1730868821.01 epoch sec; seqNo:2563 2024-11-06T04:53:41Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 1.51 psi 2024-11-06T04:53:41Z- handleDepth()- Depth_Keller msg: 1730868821.05 epoch sec; seqNo:2732; depth 50.35 2024-11-06T04:53:41Z- handleDVL() - DVL msg epochMillisec: 1730868821.07 2024-11-06T04:53:41Z- handleDvl() - RDI_Pathfinder msg: 1730868821.07 sec; seqNo:1003; #beams:4 2024-11-06T04:53:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.40 , 32.11 , 39.75 , 33.81 2024-11-06T04:53:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.02 , 0.03 2024-11-06T04:53:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868821.08 epoch sec; seqNo:2732; 4061729.03 north; 594852.43 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:53:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868818.61 2024-11-06T04:53:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:41Z- waiting for 1730868822.61, time is 1730868821.08 2024-11-06T04:53:41Z- AHRS_M2 msg: 1730868821.42 epoch sec; seqNo:2564 2024-11-06T04:53:41Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.52 psi 2024-11-06T04:53:41Z- handleDepth()- Depth_Keller msg: 1730868821.42 epoch sec; seqNo:2733; depth 50.36 2024-11-06T04:53:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868821.44 epoch sec; seqNo:2733; 4061729.03 north; 594852.43 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:53:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868818.61 2024-11-06T04:53:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:41Z- waiting for 1730868822.61, time is 1730868821.44 2024-11-06T04:53:41Z- AHRS_M2 msg: 1730868821.83 epoch sec; seqNo:2565 2024-11-06T04:53:41Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.52 psi 2024-11-06T04:53:41Z- handleDepth()- Depth_Keller msg: 1730868821.84 epoch sec; seqNo:2734; depth 50.36 2024-11-06T04:53:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868821.85 epoch sec; seqNo:2734; 4061729.03 north; 594852.43 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:53:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868818.61 2024-11-06T04:53:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:41Z- waiting for 1730868822.61, time is 1730868821.85 2024-11-06T04:53:42Z- AHRS_M2 msg: 1730868822.23 epoch sec; seqNo:2566 2024-11-06T04:53:42Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.52 psi 2024-11-06T04:53:42Z- handleDepth()- Depth_Keller msg: 1730868822.24 epoch sec; seqNo:2735; depth 50.36 2024-11-06T04:53:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868822.25 epoch sec; seqNo:2735; 4061729.03 north; 594852.43 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:53:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868818.61 2024-11-06T04:53:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:42Z- waiting for 1730868822.61, time is 1730868822.25 2024-11-06T04:53:42Z- AHRS_M2 msg: 1730868822.63 epoch sec; seqNo:2567 2024-11-06T04:53:42Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.52 psi 2024-11-06T04:53:42Z- handleDepth()- Depth_Keller msg: 1730868822.64 epoch sec; seqNo:2736; depth 50.37 2024-11-06T04:53:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868822.66 epoch sec; seqNo:2736; 4061729.04 north; 594853.65 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:53:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868818.61 2024-11-06T04:53:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:42Z- waiting for 1730868822.61, time is 1730868822.66 2024-11-06T04:53:42Z- LcmTrn::updateTrn() >>>> pose time 1730868822.66, meas time 1730868821.07 <<<< 2024-11-06T04:53:42Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868822.66 2024-11-06T04:53:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868821.07 sec, ping # 0; waiting for INS data more recent than 1730868818.61... 2024-11-06T04:53:42Z- TerrainNav::motionUpdate() - currEstimate = (4061729, 594853.65, 50.37) 2024-11-06T04:53:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:53:42Z- TerrainNav:: SubcloudNIS is 0.83, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:53:42Z- TNavPF::Projecting Measurements 2024-11-06T04:53:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:53:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:53:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:53:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:53:42Z- TNavFilter::4 good beams. 2024-11-06T04:53:42Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:53:42Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:53:42Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:53:42Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:53:42Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:53:42Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:53:42Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:53:42Z- TerrainMapOctree: (vehN, vehE ) = (4061728.88, 594852.21). 2024-11-06T04:53:42Z- TerrainMapOctree: Min Distance = 104.37. 2024-11-06T04:53:42Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:53:42Z- Forcing Subcloud Comparison 2024-11-06T04:53:42Z- Weighting particles with subcloud comparison 2024-11-06T04:53:42Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:42Z- meanExpectedMeasDiff: -3.107856 delta_rms_squared: 3.891164 alpha: 0.994886 2024-11-06T04:53:42Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:42Z- meanExpectedMeasDiff: -3.695190 delta_rms_squared: 2.924087 alpha: 0.993207 2024-11-06T04:53:42Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:42Z- meanExpectedMeasDiff: 1.751891 delta_rms_squared: 3.295375 alpha: 0.993967 2024-11-06T04:53:42Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:42Z- meanExpectedMeasDiff: -1.598980 delta_rms_squared: 3.115185 alpha: 0.993621 2024-11-06T04:53:42Z- Subcloud NIS: 0.818966 2024-11-06T04:53:42Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:53:42Z- TNavPF:: sumWeights = 0.035560 2024-11-06T04:53:42Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:53:42Z- TNavPF::First Term of Map Covariance Matrix is 0.12 2024-11-06T04:53:42Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T04:53:42Z- TerrainNav::Resampling particle filter... 2024-11-06T04:53:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868821.07 sec. 2024-11-06T04:53:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:53:42Z- variances([0], [2], and psi_berg) = 0.668564 0.236304 0.000000 2024-11-06T04:53:42Z- publishEstimatesDecoder() - at 1730868822.66 sec 2024-11-06T04:53:42Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:53:42Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061674.50 594857.76 62.85 2024-11-06T04:53:42Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061672.61 594858.24 63.56 lat/lon: 36.70 -121.94 2024-11-06T04:53:42Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.67 0.24 0.85 0.00 2024-11-06T04:53:43Z- AHRS_M2 msg: 1730868823.04 epoch sec; seqNo:2568 2024-11-06T04:53:43Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.52 psi 2024-11-06T04:53:43Z- handleDepth()- Depth_Keller msg: 1730868823.04 epoch sec; seqNo:2737; depth 50.37 2024-11-06T04:53:43Z- handleDVL() - DVL msg epochMillisec: 1730868823.11 2024-11-06T04:53:43Z- handleDvl() - RDI_Pathfinder msg: 1730868823.11 sec; seqNo:1004; #beams:4 2024-11-06T04:53:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.40 , 32.11 , 40.09 , 34.15 2024-11-06T04:53:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , -0.04 2024-11-06T04:53:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868823.14 epoch sec; seqNo:2737; 4061729.04 north; 594853.65 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:53:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868822.66 2024-11-06T04:53:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:43Z- waiting for 1730868826.66, time is 1730868823.14 2024-11-06T04:53:43Z- AHRS_M2 msg: 1730868823.45 epoch sec; seqNo:2569 2024-11-06T04:53:43Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 1.52 psi 2024-11-06T04:53:43Z- handleDepth()- Depth_Keller msg: 1730868823.45 epoch sec; seqNo:2738; depth 50.38 2024-11-06T04:53:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868823.47 epoch sec; seqNo:2738; 4061729.04 north; 594853.65 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:53:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868822.66 2024-11-06T04:53:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:43Z- waiting for 1730868826.66, time is 1730868823.47 2024-11-06T04:53:43Z- AHRS_M2 msg: 1730868823.84 epoch sec; seqNo:2570 2024-11-06T04:53:43Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.52 psi 2024-11-06T04:53:43Z- handleDepth()- Depth_Keller msg: 1730868823.85 epoch sec; seqNo:2739; depth 50.37 2024-11-06T04:53:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868823.87 epoch sec; seqNo:2739; 4061729.06 north; 594854.87 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:53:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868822.66 2024-11-06T04:53:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:44Z- waiting for 1730868826.66, time is 1730868823.87 2024-11-06T04:53:44Z- AHRS_M2 msg: 1730868824.25 epoch sec; seqNo:2571 2024-11-06T04:53:44Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.52 psi 2024-11-06T04:53:44Z- handleDepth()- Depth_Keller msg: 1730868824.25 epoch sec; seqNo:2740; depth 50.37 2024-11-06T04:53:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868824.26 epoch sec; seqNo:2740; 4061729.06 north; 594854.87 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:53:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868822.66 2024-11-06T04:53:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:44Z- waiting for 1730868826.66, time is 1730868824.26 2024-11-06T04:53:44Z- AHRS_M2 msg: 1730868824.65 epoch sec; seqNo:2572 2024-11-06T04:53:44Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.52 psi 2024-11-06T04:53:44Z- handleDepth()- Depth_Keller msg: 1730868824.66 epoch sec; seqNo:2741; depth 50.37 2024-11-06T04:53:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868824.67 epoch sec; seqNo:2741; 4061729.06 north; 594854.87 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:53:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868822.66 2024-11-06T04:53:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:44Z- waiting for 1730868826.66, time is 1730868824.67 2024-11-06T04:53:45Z- AHRS_M2 msg: 1730868825.07 epoch sec; seqNo:2573 2024-11-06T04:53:45Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.52 psi 2024-11-06T04:53:45Z- handleDepth()- Depth_Keller msg: 1730868825.07 epoch sec; seqNo:2742; depth 50.37 2024-11-06T04:53:45Z- handleDVL() - DVL msg epochMillisec: 1730868825.09 2024-11-06T04:53:45Z- handleDvl() - RDI_Pathfinder msg: 1730868825.09 sec; seqNo:1005; #beams:4 2024-11-06T04:53:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.40 , 31.77 , 39.75 , 33.81 2024-11-06T04:53:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , -0.08 2024-11-06T04:53:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868825.12 epoch sec; seqNo:2742; 4061729.07 north; 594856.09 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:53:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868822.66 2024-11-06T04:53:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:45Z- waiting for 1730868826.66, time is 1730868825.12 2024-11-06T04:53:45Z- AHRS_M2 msg: 1730868825.46 epoch sec; seqNo:2574 2024-11-06T04:53:45Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.52 psi 2024-11-06T04:53:45Z- handleDepth()- Depth_Keller msg: 1730868825.46 epoch sec; seqNo:2743; depth 50.37 2024-11-06T04:53:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868825.49 epoch sec; seqNo:2743; 4061729.07 north; 594856.09 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:53:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868822.66 2024-11-06T04:53:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:45Z- waiting for 1730868826.66, time is 1730868825.49 2024-11-06T04:53:45Z- AHRS_M2 msg: 1730868825.87 epoch sec; seqNo:2575 2024-11-06T04:53:45Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 1.52 psi 2024-11-06T04:53:45Z- handleDepth()- Depth_Keller msg: 1730868825.87 epoch sec; seqNo:2744; depth 50.37 2024-11-06T04:53:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868825.89 epoch sec; seqNo:2744; 4061729.07 north; 594856.09 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:53:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868822.66 2024-11-06T04:53:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:46Z- waiting for 1730868826.66, time is 1730868825.89 2024-11-06T04:53:46Z- AHRS_M2 msg: 1730868826.27 epoch sec; seqNo:2576 2024-11-06T04:53:46Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:53:46Z- handleDepth()- Depth_Keller msg: 1730868826.28 epoch sec; seqNo:2745; depth 50.38 2024-11-06T04:53:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868826.29 epoch sec; seqNo:2745; 4061729.07 north; 594856.09 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:53:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868822.66 2024-11-06T04:53:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:46Z- waiting for 1730868826.66, time is 1730868826.29 2024-11-06T04:53:46Z- AHRS_M2 msg: 1730868826.68 epoch sec; seqNo:2577 2024-11-06T04:53:46Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:53:46Z- handleDepth()- Depth_Keller msg: 1730868826.69 epoch sec; seqNo:2746; depth 50.39 2024-11-06T04:53:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868826.75 epoch sec; seqNo:2746; 4061729.08 north; 594857.31 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:53:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868822.66 2024-11-06T04:53:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:46Z- waiting for 1730868826.66, time is 1730868826.75 2024-11-06T04:53:46Z- LcmTrn::updateTrn() >>>> pose time 1730868826.75, meas time 1730868825.09 <<<< 2024-11-06T04:53:46Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868826.75 2024-11-06T04:53:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868825.09 sec, ping # 0; waiting for INS data more recent than 1730868822.66... 2024-11-06T04:53:46Z- TerrainNav::motionUpdate() - currEstimate = (4061729, 594857.31, 50.39) 2024-11-06T04:53:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:53:46Z- TerrainNav:: SubcloudNIS is 0.82, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:53:46Z- TNavPF::Projecting Measurements 2024-11-06T04:53:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:53:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:53:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:53:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:53:46Z- TNavFilter::4 good beams. 2024-11-06T04:53:46Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:53:46Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:53:46Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:53:46Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:53:46Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:53:46Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:53:46Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:53:46Z- TerrainMapOctree: (vehN, vehE ) = (4061729.07, 594855.82). 2024-11-06T04:53:46Z- TerrainMapOctree: Min Distance = 107.87. 2024-11-06T04:53:46Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:53:46Z- Forcing Subcloud Comparison 2024-11-06T04:53:46Z- Weighting particles with subcloud comparison 2024-11-06T04:53:46Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:46Z- meanExpectedMeasDiff: -1.537941 delta_rms_squared: 1.806440 alpha: 0.989050 2024-11-06T04:53:46Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:46Z- meanExpectedMeasDiff: -2.722303 delta_rms_squared: 1.313883 alpha: 0.985006 2024-11-06T04:53:46Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:46Z- meanExpectedMeasDiff: 3.070403 delta_rms_squared: 1.492979 alpha: 0.986781 2024-11-06T04:53:46Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:46Z- meanExpectedMeasDiff: -1.891374 delta_rms_squared: 0.820664 alpha: 0.976209 2024-11-06T04:53:46Z- Subcloud NIS: 0.821983 2024-11-06T04:53:46Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:53:46Z- TNavPF:: sumWeights = 0.040460 2024-11-06T04:53:46Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:53:47Z- TNavPF::First Term of Map Covariance Matrix is 0.07 2024-11-06T04:53:47Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T04:53:47Z- TerrainNav::Resampling particle filter... 2024-11-06T04:53:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868825.09 sec. 2024-11-06T04:53:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:53:47Z- variances([0], [2], and psi_berg) = 0.687326 0.248867 0.000000 2024-11-06T04:53:47Z- publishEstimatesDecoder() - at 1730868826.75 sec 2024-11-06T04:53:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:53:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061674.58 594861.35 63.00 2024-11-06T04:53:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061672.65 594861.87 64.03 lat/lon: 36.70 -121.94 2024-11-06T04:53:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.69 0.25 0.76 0.00 2024-11-06T04:53:47Z- AHRS_M2 msg: 1730868827.08 epoch sec; seqNo:2578 2024-11-06T04:53:47Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.52 psi 2024-11-06T04:53:47Z- handleDepth()- Depth_Keller msg: 1730868827.09 epoch sec; seqNo:2747; depth 50.40 2024-11-06T04:53:47Z- handleDVL() - DVL msg epochMillisec: 1730868827.14 2024-11-06T04:53:47Z- handleDvl() - RDI_Pathfinder msg: 1730868827.14 sec; seqNo:1006; #beams:4 2024-11-06T04:53:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.22 , 32.11 , 39.92 , 33.64 2024-11-06T04:53:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.05 , -0.04 2024-11-06T04:53:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868827.16 epoch sec; seqNo:2747; 4061729.08 north; 594857.31 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T04:53:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868826.75 2024-11-06T04:53:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:47Z- waiting for 1730868830.75, time is 1730868827.16 2024-11-06T04:53:47Z- AHRS_M2 msg: 1730868827.48 epoch sec; seqNo:2579 2024-11-06T04:53:47Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 1.51 psi 2024-11-06T04:53:47Z- handleDepth()- Depth_Keller msg: 1730868827.49 epoch sec; seqNo:2748; depth 50.42 2024-11-06T04:53:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868827.50 epoch sec; seqNo:2748; 4061729.46 north; 594857.30 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T04:53:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868826.75 2024-11-06T04:53:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:47Z- waiting for 1730868830.75, time is 1730868827.50 2024-11-06T04:53:47Z- AHRS_M2 msg: 1730868827.88 epoch sec; seqNo:2580 2024-11-06T04:53:47Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 1.51 psi 2024-11-06T04:53:47Z- handleDepth()- Depth_Keller msg: 1730868827.89 epoch sec; seqNo:2749; depth 50.43 2024-11-06T04:53:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868827.90 epoch sec; seqNo:2749; 4061729.48 north; 594858.52 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:53:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868826.75 2024-11-06T04:53:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:48Z- waiting for 1730868830.75, time is 1730868827.90 2024-11-06T04:53:48Z- AHRS_M2 msg: 1730868828.29 epoch sec; seqNo:2581 2024-11-06T04:53:48Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.51 psi 2024-11-06T04:53:48Z- handleDepth()- Depth_Keller msg: 1730868828.30 epoch sec; seqNo:2750; depth 50.43 2024-11-06T04:53:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868828.31 epoch sec; seqNo:2750; 4061729.48 north; 594858.52 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:53:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868826.75 2024-11-06T04:53:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:48Z- waiting for 1730868830.75, time is 1730868828.31 2024-11-06T04:53:48Z- AHRS_M2 msg: 1730868828.73 epoch sec; seqNo:2582 2024-11-06T04:53:48Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.51 psi 2024-11-06T04:53:48Z- handleDepth()- Depth_Keller msg: 1730868828.75 epoch sec; seqNo:2751; depth 50.44 2024-11-06T04:53:48Z- handleDVL() - DVL msg epochMillisec: 1730868828.77 2024-11-06T04:53:48Z- handleDvl() - RDI_Pathfinder msg: 1730868828.77 sec; seqNo:1007; #beams:4 2024-11-06T04:53:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.22 , 34.66 , 39.75 , 33.47 2024-11-06T04:53:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.06 , 0.04 2024-11-06T04:53:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868828.79 epoch sec; seqNo:2751; 4061729.48 north; 594858.52 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T04:53:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868826.75 2024-11-06T04:53:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:48Z- waiting for 1730868830.75, time is 1730868828.79 2024-11-06T04:53:49Z- AHRS_M2 msg: 1730868829.09 epoch sec; seqNo:2583 2024-11-06T04:53:49Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.51 psi 2024-11-06T04:53:49Z- handleDepth()- Depth_Keller msg: 1730868829.11 epoch sec; seqNo:2752; depth 50.48 2024-11-06T04:53:49Z- handleDVL() - DVL msg epochMillisec: 1730868829.11 2024-11-06T04:53:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:49Z- handleDvl() - RDI_Pathfinder msg: 1730868829.11 sec; seqNo:1008; #beams:0 2024-11-06T04:53:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.06 , 0.04 2024-11-06T04:53:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868829.14 epoch sec; seqNo:2752; 4061729.48 north; 594858.52 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T04:53:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868826.75 2024-11-06T04:53:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:49Z- waiting for 1730868830.75, time is 1730868829.14 2024-11-06T04:53:49Z- AHRS_M2 msg: 1730868829.50 epoch sec; seqNo:2584 2024-11-06T04:53:49Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.50 psi 2024-11-06T04:53:49Z- handleDepth()- Depth_Keller msg: 1730868829.51 epoch sec; seqNo:2753; depth 50.49 2024-11-06T04:53:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868829.52 epoch sec; seqNo:2753; 4061729.48 north; 594858.52 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T04:53:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868826.75 2024-11-06T04:53:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:49Z- waiting for 1730868830.75, time is 1730868829.52 2024-11-06T04:53:49Z- AHRS_M2 msg: 1730868829.90 epoch sec; seqNo:2585 2024-11-06T04:53:49Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.50 psi 2024-11-06T04:53:49Z- handleDepth()- Depth_Keller msg: 1730868829.91 epoch sec; seqNo:2754; depth 50.51 2024-11-06T04:53:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868829.92 epoch sec; seqNo:2754; 4061729.49 north; 594859.74 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T04:53:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868826.75 2024-11-06T04:53:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:50Z- waiting for 1730868830.75, time is 1730868829.92 2024-11-06T04:53:50Z- AHRS_M2 msg: 1730868830.31 epoch sec; seqNo:2586 2024-11-06T04:53:50Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.50 psi 2024-11-06T04:53:50Z- handleDepth()- Depth_Keller msg: 1730868830.33 epoch sec; seqNo:2755; depth 50.52 2024-11-06T04:53:50Z- handleDVL() - DVL msg epochMillisec: 1730868830.33 2024-11-06T04:53:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:50Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:50Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:50Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:50Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:50Z- handleDvl() - RDI_Pathfinder msg: 1730868830.33 sec; seqNo:1009; #beams:0 2024-11-06T04:53:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.06 , 0.04 2024-11-06T04:53:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868830.34 epoch sec; seqNo:2755; 4061729.49 north; 594859.74 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T04:53:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868826.75 2024-11-06T04:53:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:50Z- waiting for 1730868830.75, time is 1730868830.34 2024-11-06T04:53:50Z- AHRS_M2 msg: 1730868830.75 epoch sec; seqNo:2587 2024-11-06T04:53:50Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.50 psi 2024-11-06T04:53:50Z- handleDepth()- Depth_Keller msg: 1730868830.76 epoch sec; seqNo:2756; depth 50.54 2024-11-06T04:53:50Z- handleDVL() - DVL msg epochMillisec: 1730868830.78 2024-11-06T04:53:50Z- handleDvl() - RDI_Pathfinder msg: 1730868830.78 sec; seqNo:1010; #beams:4 2024-11-06T04:53:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.56 , 35.85 , 40.09 , 33.64 2024-11-06T04:53:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.10 2024-11-06T04:53:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868830.81 epoch sec; seqNo:2756; 4061729.49 north; 594859.74 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T04:53:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868826.75 2024-11-06T04:53:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:50Z- waiting for 1730868830.75, time is 1730868830.81 2024-11-06T04:53:50Z- LcmTrn::updateTrn() >>>> pose time 1730868830.81, meas time 1730868830.78 <<<< 2024-11-06T04:53:50Z- LcmTrn::updateTrn() equating pose 1730868830.78 and meas times 1730868830.78 2024-11-06T04:53:50Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868830.78 2024-11-06T04:53:50Z- TerrainNav::motionUpdate() - currEstimate = (4061729, 594859.74, 50.54) 2024-11-06T04:53:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:53:50Z- TerrainNav:: SubcloudNIS is 0.82, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:53:50Z- TNavPF::Projecting Measurements 2024-11-06T04:53:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:53:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:53:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:53:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:53:50Z- TNavFilter::4 good beams. 2024-11-06T04:53:50Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:53:50Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:53:50Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:53:50Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:53:50Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:53:50Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:53:50Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:53:50Z- TerrainMapOctree: (vehN, vehE ) = (4061729.49, 594859.74). 2024-11-06T04:53:50Z- TerrainMapOctree: Min Distance = 111.64. 2024-11-06T04:53:50Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:53:51Z- Forcing Subcloud Comparison 2024-11-06T04:53:51Z- Weighting particles with subcloud comparison 2024-11-06T04:53:51Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:51Z- meanExpectedMeasDiff: -0.239535 delta_rms_squared: 1.605977 alpha: 0.987700 2024-11-06T04:53:51Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:51Z- meanExpectedMeasDiff: 0.833379 delta_rms_squared: 1.151493 alpha: 0.982928 2024-11-06T04:53:51Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:51Z- meanExpectedMeasDiff: 2.819880 delta_rms_squared: 1.360014 alpha: 0.985507 2024-11-06T04:53:51Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:51Z- meanExpectedMeasDiff: -2.537198 delta_rms_squared: 1.302584 alpha: 0.984878 2024-11-06T04:53:51Z- Subcloud NIS: 0.836055 2024-11-06T04:53:51Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:53:51Z- TNavPF:: sumWeights = 0.099317 2024-11-06T04:53:51Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:53:51Z- TNavPF::First Term of Map Covariance Matrix is 0.07 2024-11-06T04:53:51Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T04:53:51Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868830.78 sec, ping # 0. 2024-11-06T04:53:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:53:51Z- variances([0], [2], and psi_berg) = 0.598528 0.189083 0.000000 2024-11-06T04:53:51Z- publishEstimatesDecoder() - at 1730868830.78 sec 2024-11-06T04:53:51Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:53:51Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061671.38 594864.92 64.15 2024-11-06T04:53:51Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061672.99 594864.35 64.16 lat/lon: 36.70 -121.94 2024-11-06T04:53:51Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.60 0.19 0.42 0.00 2024-11-06T04:53:51Z- AHRS_M2 msg: 1730868831.12 epoch sec; seqNo:2588 2024-11-06T04:53:51Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.50 psi 2024-11-06T04:53:51Z- handleDepth()- Depth_Keller msg: 1730868831.14 epoch sec; seqNo:2757; depth 50.57 2024-11-06T04:53:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868831.17 epoch sec; seqNo:2757; 4061729.88 north; 594860.96 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T04:53:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868830.78 2024-11-06T04:53:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:51Z- waiting for 1730868834.78, time is 1730868831.17 2024-11-06T04:53:51Z- AHRS_M2 msg: 1730868831.52 epoch sec; seqNo:2589 2024-11-06T04:53:51Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.50 psi 2024-11-06T04:53:51Z- handleDepth()- Depth_Keller msg: 1730868831.53 epoch sec; seqNo:2758; depth 50.58 2024-11-06T04:53:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868831.54 epoch sec; seqNo:2758; 4061729.88 north; 594860.96 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T04:53:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868830.78 2024-11-06T04:53:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:51Z- waiting for 1730868834.78, time is 1730868831.54 2024-11-06T04:53:51Z- AHRS_M2 msg: 1730868831.93 epoch sec; seqNo:2590 2024-11-06T04:53:51Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.49 psi 2024-11-06T04:53:51Z- handleDepth()- Depth_Keller msg: 1730868831.95 epoch sec; seqNo:2759; depth 50.60 2024-11-06T04:53:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868831.96 epoch sec; seqNo:2759; 4061729.88 north; 594860.96 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T04:53:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868830.78 2024-11-06T04:53:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:52Z- waiting for 1730868834.78, time is 1730868831.96 2024-11-06T04:53:52Z- AHRS_M2 msg: 1730868832.33 epoch sec; seqNo:2591 2024-11-06T04:53:52Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.49 psi 2024-11-06T04:53:52Z- handleDepth()- Depth_Keller msg: 1730868832.34 epoch sec; seqNo:2760; depth 50.60 2024-11-06T04:53:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868832.35 epoch sec; seqNo:2760; 4061729.88 north; 594860.96 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T04:53:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868830.78 2024-11-06T04:53:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:52Z- waiting for 1730868834.78, time is 1730868832.35 2024-11-06T04:53:52Z- AHRS_M2 msg: 1730868832.76 epoch sec; seqNo:2592 2024-11-06T04:53:52Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.49 psi 2024-11-06T04:53:52Z- handleDepth()- Depth_Keller msg: 1730868832.76 epoch sec; seqNo:2761; depth 50.64 2024-11-06T04:53:52Z- handleDVL() - DVL msg epochMillisec: 1730868832.79 2024-11-06T04:53:52Z- handleDvl() - RDI_Pathfinder msg: 1730868832.79 sec; seqNo:1011; #beams:4 2024-11-06T04:53:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 38.74 , 36.02 , 40.60 , 34.15 2024-11-06T04:53:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.00 , 0.11 2024-11-06T04:53:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868832.80 epoch sec; seqNo:2761; 4061729.90 north; 594862.18 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T04:53:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868830.78 2024-11-06T04:53:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:52Z- waiting for 1730868834.78, time is 1730868832.80 2024-11-06T04:53:53Z- AHRS_M2 msg: 1730868833.14 epoch sec; seqNo:2593 2024-11-06T04:53:53Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.50 psi 2024-11-06T04:53:53Z- handleDepth()- Depth_Keller msg: 1730868833.14 epoch sec; seqNo:2762; depth 50.63 2024-11-06T04:53:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868833.17 epoch sec; seqNo:2762; 4061729.90 north; 594862.18 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T04:53:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868830.78 2024-11-06T04:53:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:53Z- waiting for 1730868834.78, time is 1730868833.17 2024-11-06T04:53:53Z- AHRS_M2 msg: 1730868833.54 epoch sec; seqNo:2594 2024-11-06T04:53:53Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.50 psi 2024-11-06T04:53:53Z- handleDepth()- Depth_Keller msg: 1730868833.55 epoch sec; seqNo:2763; depth 50.63 2024-11-06T04:53:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868833.57 epoch sec; seqNo:2763; 4061729.90 north; 594862.18 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T04:53:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868830.78 2024-11-06T04:53:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:53Z- waiting for 1730868834.78, time is 1730868833.57 2024-11-06T04:53:53Z- AHRS_M2 msg: 1730868833.95 epoch sec; seqNo:2595 2024-11-06T04:53:53Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 1.50 psi 2024-11-06T04:53:53Z- handleDepth()- Depth_Keller msg: 1730868833.96 epoch sec; seqNo:2764; depth 50.63 2024-11-06T04:53:53Z- handleDVL() - DVL msg epochMillisec: 1730868833.96 2024-11-06T04:53:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:53Z- handleDvl() - RDI_Pathfinder msg: 1730868833.96 sec; seqNo:1012; #beams:0 2024-11-06T04:53:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.00 , 0.11 2024-11-06T04:53:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868833.97 epoch sec; seqNo:2764; 4061729.91 north; 594863.40 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T04:53:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868830.78 2024-11-06T04:53:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:54Z- waiting for 1730868834.78, time is 1730868833.97 2024-11-06T04:53:54Z- AHRS_M2 msg: 1730868834.35 epoch sec; seqNo:2596 2024-11-06T04:53:54Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:53:54Z- handleDepth()- Depth_Keller msg: 1730868834.36 epoch sec; seqNo:2765; depth 50.64 2024-11-06T04:53:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868834.37 epoch sec; seqNo:2765; 4061729.91 north; 594863.40 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T04:53:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868830.78 2024-11-06T04:53:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:54Z- waiting for 1730868834.78, time is 1730868834.37 2024-11-06T04:53:54Z- AHRS_M2 msg: 1730868834.76 epoch sec; seqNo:2597 2024-11-06T04:53:54Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.50 psi 2024-11-06T04:53:54Z- handleDepth()- Depth_Keller msg: 1730868834.77 epoch sec; seqNo:2766; depth 50.64 2024-11-06T04:53:54Z- handleDVL() - DVL msg epochMillisec: 1730868834.79 2024-11-06T04:53:54Z- handleDvl() - RDI_Pathfinder msg: 1730868834.79 sec; seqNo:1013; #beams:4 2024-11-06T04:53:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.24 , 36.36 , 41.62 , 34.32 2024-11-06T04:53:54Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.06 , 0.01 2024-11-06T04:53:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868834.81 epoch sec; seqNo:2766; 4061729.91 north; 594863.40 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T04:53:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868830.78 2024-11-06T04:53:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:54Z- waiting for 1730868834.78, time is 1730868834.81 2024-11-06T04:53:54Z- LcmTrn::updateTrn() >>>> pose time 1730868834.81, meas time 1730868834.79 <<<< 2024-11-06T04:53:54Z- LcmTrn::updateTrn() equating pose 1730868834.79 and meas times 1730868834.79 2024-11-06T04:53:54Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868834.79 2024-11-06T04:53:54Z- TerrainNav::motionUpdate() - currEstimate = (4061730, 594863.40, 50.64) 2024-11-06T04:53:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:53:54Z- TerrainNav:: SubcloudNIS is 0.84, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:53:54Z- TNavPF::Projecting Measurements 2024-11-06T04:53:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:53:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:53:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:53:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:53:54Z- TNavFilter::4 good beams. 2024-11-06T04:53:54Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:53:54Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:53:54Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:53:54Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:53:54Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:53:54Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:53:54Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:53:54Z- TerrainMapOctree: (vehN, vehE ) = (4061729.91, 594863.40). 2024-11-06T04:53:54Z- TerrainMapOctree: Min Distance = 115.17. 2024-11-06T04:53:54Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:53:55Z- Forcing Subcloud Comparison 2024-11-06T04:53:55Z- Weighting particles with subcloud comparison 2024-11-06T04:53:55Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:55Z- meanExpectedMeasDiff: 0.245192 delta_rms_squared: 1.080853 alpha: 0.981832 2024-11-06T04:53:55Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:55Z- meanExpectedMeasDiff: 1.535622 delta_rms_squared: 0.754627 alpha: 0.974181 2024-11-06T04:53:55Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:55Z- meanExpectedMeasDiff: 4.242105 delta_rms_squared: 0.871215 alpha: 0.977559 2024-11-06T04:53:55Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:55Z- meanExpectedMeasDiff: -2.907209 delta_rms_squared: 0.913012 alpha: 0.978564 2024-11-06T04:53:55Z- Subcloud NIS: 0.858949 2024-11-06T04:53:55Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:53:55Z- TNavPF:: sumWeights = 0.025906 2024-11-06T04:53:55Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:53:55Z- TNavPF::First Term of Map Covariance Matrix is 0.02 2024-11-06T04:53:55Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T04:53:55Z- TerrainNav::Resampling particle filter... 2024-11-06T04:53:55Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868834.79 sec, ping # 0. 2024-11-06T04:53:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:53:55Z- variances([0], [2], and psi_berg) = 0.617505 0.201866 0.000000 2024-11-06T04:53:55Z- publishEstimatesDecoder() - at 1730868834.79 sec 2024-11-06T04:53:55Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:53:55Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061675.33 594867.51 63.88 2024-11-06T04:53:55Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061673.42 594868.00 64.04 lat/lon: 36.70 -121.94 2024-11-06T04:53:55Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.62 0.20 0.36 0.00 2024-11-06T04:53:55Z- AHRS_M2 msg: 1730868835.16 epoch sec; seqNo:2598 2024-11-06T04:53:55Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.51 psi 2024-11-06T04:53:55Z- handleDepth()- Depth_Keller msg: 1730868835.17 epoch sec; seqNo:2767; depth 50.65 2024-11-06T04:53:55Z- handleDVL() - DVL msg epochMillisec: 1730868835.17 2024-11-06T04:53:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:53:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:53:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:53:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:53:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:53:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:53:55Z- handleDvl() - RDI_Pathfinder msg: 1730868835.17 sec; seqNo:1014; #beams:0 2024-11-06T04:53:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:53:55Z- handleDvl() - RDI_Pathfinder msg: velocities 1.03 , 0.06 , 0.01 2024-11-06T04:53:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868835.18 epoch sec; seqNo:2767; 4061729.92 north; 594864.62 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T04:53:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868834.79 2024-11-06T04:53:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:55Z- waiting for 1730868838.79, time is 1730868835.18 2024-11-06T04:53:55Z- AHRS_M2 msg: 1730868835.56 epoch sec; seqNo:2599 2024-11-06T04:53:55Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.51 psi 2024-11-06T04:53:55Z- handleDepth()- Depth_Keller msg: 1730868835.57 epoch sec; seqNo:2768; depth 50.65 2024-11-06T04:53:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868835.58 epoch sec; seqNo:2768; 4061729.92 north; 594864.62 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T04:53:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868834.79 2024-11-06T04:53:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:55Z- waiting for 1730868838.79, time is 1730868835.58 2024-11-06T04:53:55Z- AHRS_M2 msg: 1730868835.97 epoch sec; seqNo:2600 2024-11-06T04:53:55Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.51 psi 2024-11-06T04:53:56Z- handleDepth()- Depth_Keller msg: 1730868836.00 epoch sec; seqNo:2769; depth 50.63 2024-11-06T04:53:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868836.02 epoch sec; seqNo:2769; 4061729.92 north; 594864.62 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T04:53:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868834.79 2024-11-06T04:53:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:56Z- waiting for 1730868838.79, time is 1730868836.02 2024-11-06T04:53:56Z- AHRS_M2 msg: 1730868836.37 epoch sec; seqNo:2601 2024-11-06T04:53:56Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.51 psi 2024-11-06T04:53:56Z- handleDepth()- Depth_Keller msg: 1730868836.38 epoch sec; seqNo:2770; depth 50.62 2024-11-06T04:53:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868836.39 epoch sec; seqNo:2770; 4061729.94 north; 594865.84 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T04:53:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868834.79 2024-11-06T04:53:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:56Z- waiting for 1730868838.79, time is 1730868836.39 2024-11-06T04:53:56Z- AHRS_M2 msg: 1730868836.78 epoch sec; seqNo:2602 2024-11-06T04:53:56Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 1.52 psi 2024-11-06T04:53:56Z- handleDepth()- Depth_Keller msg: 1730868836.78 epoch sec; seqNo:2771; depth 50.60 2024-11-06T04:53:56Z- handleDVL() - DVL msg epochMillisec: 1730868836.80 2024-11-06T04:53:56Z- handleDvl() - RDI_Pathfinder msg: 1730868836.80 sec; seqNo:1015; #beams:4 2024-11-06T04:53:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.26 , 36.70 , 42.64 , 34.32 2024-11-06T04:53:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.02 , -0.09 2024-11-06T04:53:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868836.85 epoch sec; seqNo:2771; 4061729.94 north; 594865.84 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T04:53:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868834.79 2024-11-06T04:53:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:56Z- waiting for 1730868838.79, time is 1730868836.85 2024-11-06T04:53:57Z- AHRS_M2 msg: 1730868837.18 epoch sec; seqNo:2603 2024-11-06T04:53:57Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.52 psi 2024-11-06T04:53:57Z- handleDepth()- Depth_Keller msg: 1730868837.19 epoch sec; seqNo:2772; depth 50.60 2024-11-06T04:53:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868837.20 epoch sec; seqNo:2772; 4061729.94 north; 594865.84 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T04:53:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868834.79 2024-11-06T04:53:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:57Z- waiting for 1730868838.79, time is 1730868837.20 2024-11-06T04:53:57Z- AHRS_M2 msg: 1730868837.58 epoch sec; seqNo:2604 2024-11-06T04:53:57Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.52 psi 2024-11-06T04:53:57Z- handleDepth()- Depth_Keller msg: 1730868837.59 epoch sec; seqNo:2773; depth 50.59 2024-11-06T04:53:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868837.61 epoch sec; seqNo:2773; 4061729.95 north; 594867.06 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T04:53:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868834.79 2024-11-06T04:53:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:57Z- waiting for 1730868838.79, time is 1730868837.61 2024-11-06T04:53:57Z- AHRS_M2 msg: 1730868837.99 epoch sec; seqNo:2605 2024-11-06T04:53:57Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.52 psi 2024-11-06T04:53:57Z- handleDepth()- Depth_Keller msg: 1730868837.99 epoch sec; seqNo:2774; depth 50.58 2024-11-06T04:53:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868838.00 epoch sec; seqNo:2774; 4061729.95 north; 594867.06 east; 36.70 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T04:53:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868834.79 2024-11-06T04:53:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:58Z- waiting for 1730868838.79, time is 1730868838.00 2024-11-06T04:53:58Z- AHRS_M2 msg: 1730868838.39 epoch sec; seqNo:2606 2024-11-06T04:53:58Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.52 psi 2024-11-06T04:53:58Z- handleDepth()- Depth_Keller msg: 1730868838.40 epoch sec; seqNo:2775; depth 50.57 2024-11-06T04:53:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868838.41 epoch sec; seqNo:2775; 4061729.95 north; 594867.06 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T04:53:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868834.79 2024-11-06T04:53:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:58Z- waiting for 1730868838.79, time is 1730868838.41 2024-11-06T04:53:58Z- AHRS_M2 msg: 1730868838.81 epoch sec; seqNo:2607 2024-11-06T04:53:58Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.52 psi 2024-11-06T04:53:58Z- handleDepth()- Depth_Keller msg: 1730868838.81 epoch sec; seqNo:2776; depth 50.55 2024-11-06T04:53:58Z- handleDVL() - DVL msg epochMillisec: 1730868838.84 2024-11-06T04:53:58Z- handleDvl() - RDI_Pathfinder msg: 1730868838.84 sec; seqNo:1016; #beams:4 2024-11-06T04:53:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.54 , 36.44 , 42.61 , 34.20 2024-11-06T04:53:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.02 , -0.13 2024-11-06T04:53:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868838.86 epoch sec; seqNo:2776; 4061729.96 north; 594868.28 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:53:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:53:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868834.79 2024-11-06T04:53:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:58Z- waiting for 1730868838.79, time is 1730868838.86 2024-11-06T04:53:58Z- LcmTrn::updateTrn() >>>> pose time 1730868838.86, meas time 1730868838.84 <<<< 2024-11-06T04:53:58Z- LcmTrn::updateTrn() equating pose 1730868838.84 and meas times 1730868838.84 2024-11-06T04:53:58Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868838.84 2024-11-06T04:53:58Z- TerrainNav::motionUpdate() - currEstimate = (4061730, 594868.28, 50.55) 2024-11-06T04:53:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:53:58Z- TerrainNav:: SubcloudNIS is 0.86, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:53:58Z- TNavPF::Projecting Measurements 2024-11-06T04:53:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:53:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:53:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:53:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:53:58Z- TNavFilter::4 good beams. 2024-11-06T04:53:58Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:53:58Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:53:58Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:53:58Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:53:58Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:53:58Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:53:58Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:53:58Z- TerrainMapOctree: (vehN, vehE ) = (4061729.96, 594868.28). 2024-11-06T04:53:58Z- TerrainMapOctree: Min Distance = 119.97. 2024-11-06T04:53:58Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:53:59Z- Forcing Subcloud Comparison 2024-11-06T04:53:59Z- Weighting particles with subcloud comparison 2024-11-06T04:53:59Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:59Z- meanExpectedMeasDiff: 3.840094 delta_rms_squared: 0.939358 alpha: 0.979153 2024-11-06T04:53:59Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:59Z- meanExpectedMeasDiff: 1.411795 delta_rms_squared: 0.675436 alpha: 0.971241 2024-11-06T04:53:59Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:59Z- meanExpectedMeasDiff: 4.740955 delta_rms_squared: 0.790994 alpha: 0.975339 2024-11-06T04:53:59Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:53:59Z- meanExpectedMeasDiff: -3.361917 delta_rms_squared: 0.828541 alpha: 0.976430 2024-11-06T04:53:59Z- Subcloud NIS: 0.879144 2024-11-06T04:53:59Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:53:59Z- TNavPF:: sumWeights = 0.003698 2024-11-06T04:53:59Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:53:59Z- TNavPF::First Term of Map Covariance Matrix is 0.04 2024-11-06T04:53:59Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:53:59Z- TerrainNav::Resampling particle filter... 2024-11-06T04:53:59Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868838.84 sec, ping # 0. 2024-11-06T04:53:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:53:59Z- variances([0], [2], and psi_berg) = 0.670206 0.216620 0.000000 2024-11-06T04:53:59Z- publishEstimatesDecoder() - at 1730868838.84 sec 2024-11-06T04:53:59Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:53:59Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061675.26 594872.46 64.21 2024-11-06T04:53:59Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061673.46 594872.87 63.46 lat/lon: 36.70 -121.94 2024-11-06T04:53:59Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.67 0.22 0.35 0.00 2024-11-06T04:53:59Z- AHRS_M2 msg: 1730868839.22 epoch sec; seqNo:2608 2024-11-06T04:53:59Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.52 psi 2024-11-06T04:53:59Z- handleDepth()- Depth_Keller msg: 1730868839.23 epoch sec; seqNo:2777; depth 50.55 2024-11-06T04:53:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868839.26 epoch sec; seqNo:2777; 4061729.96 north; 594868.28 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:53:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868838.84 2024-11-06T04:53:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:59Z- waiting for 1730868842.84, time is 1730868839.26 2024-11-06T04:53:59Z- AHRS_M2 msg: 1730868839.60 epoch sec; seqNo:2609 2024-11-06T04:53:59Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 1.52 psi 2024-11-06T04:53:59Z- handleDepth()- Depth_Keller msg: 1730868839.61 epoch sec; seqNo:2778; depth 50.53 2024-11-06T04:53:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868839.62 epoch sec; seqNo:2778; 4061730.34 north; 594868.28 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T04:53:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:53:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868838.84 2024-11-06T04:53:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:53:59Z- waiting for 1730868842.84, time is 1730868839.62 2024-11-06T04:54:00Z- AHRS_M2 msg: 1730868840.01 epoch sec; seqNo:2610 2024-11-06T04:54:00Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 1.52 psi 2024-11-06T04:54:00Z- handleDepth()- Depth_Keller msg: 1730868840.02 epoch sec; seqNo:2779; depth 50.53 2024-11-06T04:54:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868840.02 epoch sec; seqNo:2779; 4061730.34 north; 594868.28 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T04:54:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868838.84 2024-11-06T04:54:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:00Z- waiting for 1730868842.84, time is 1730868840.02 2024-11-06T04:54:00Z- handleDepth()- Depth_Keller msg: 1730868840.43 epoch sec; seqNo:2780; depth 50.50 2024-11-06T04:54:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868840.44 epoch sec; seqNo:2780; 4061730.36 north; 594869.50 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T04:54:00Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:54:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868838.84 2024-11-06T04:54:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:00Z- waiting for 1730868842.84, time is 1730868840.44 2024-11-06T04:54:00Z- AHRS_M2 msg: 1730868840.83 epoch sec; seqNo:2611 2024-11-06T04:54:00Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.51 psi 2024-11-06T04:54:00Z- handleDepth()- Depth_Keller msg: 1730868840.84 epoch sec; seqNo:2781; depth 50.50 2024-11-06T04:54:00Z- handleDVL() - DVL msg epochMillisec: 1730868840.86 2024-11-06T04:54:00Z- handleDvl() - RDI_Pathfinder msg: 1730868840.86 sec; seqNo:1017; #beams:4 2024-11-06T04:54:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.35 , 36.25 , 42.61 , 33.83 2024-11-06T04:54:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.06 , -0.05 2024-11-06T04:54:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868840.89 epoch sec; seqNo:2781; 4061730.36 north; 594869.50 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T04:54:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868838.84 2024-11-06T04:54:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:00Z- waiting for 1730868842.84, time is 1730868840.89 2024-11-06T04:54:01Z- AHRS_M2 msg: 1730868841.22 epoch sec; seqNo:2612 2024-11-06T04:54:01Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.51 psi 2024-11-06T04:54:01Z- handleDepth()- Depth_Keller msg: 1730868841.24 epoch sec; seqNo:2782; depth 50.50 2024-11-06T04:54:01Z- handleDVL() - DVL msg epochMillisec: 1730868841.24 2024-11-06T04:54:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:54:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:54:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:54:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:54:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:54:01Z- handleDvl() - RDI_Pathfinder msg: 1730868841.24 sec; seqNo:1018; #beams:0 2024-11-06T04:54:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:54:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.06 , -0.05 2024-11-06T04:54:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868841.26 epoch sec; seqNo:2782; 4061730.36 north; 594869.50 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T04:54:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868838.84 2024-11-06T04:54:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:01Z- waiting for 1730868842.84, time is 1730868841.26 2024-11-06T04:54:01Z- AHRS_M2 msg: 1730868841.62 epoch sec; seqNo:2613 2024-11-06T04:54:01Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 1.50 psi 2024-11-06T04:54:01Z- handleDepth()- Depth_Keller msg: 1730868841.63 epoch sec; seqNo:2783; depth 50.51 2024-11-06T04:54:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868841.64 epoch sec; seqNo:2783; 4061730.36 north; 594869.50 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T04:54:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868838.84 2024-11-06T04:54:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:01Z- waiting for 1730868842.84, time is 1730868841.64 2024-11-06T04:54:02Z- AHRS_M2 msg: 1730868842.03 epoch sec; seqNo:2614 2024-11-06T04:54:02Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T04:54:02Z- handleDepth()- Depth_Keller msg: 1730868842.05 epoch sec; seqNo:2784; depth 50.50 2024-11-06T04:54:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868842.06 epoch sec; seqNo:2784; 4061730.37 north; 594870.72 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T04:54:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868838.84 2024-11-06T04:54:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:02Z- waiting for 1730868842.84, time is 1730868842.06 2024-11-06T04:54:02Z- AHRS_M2 msg: 1730868842.43 epoch sec; seqNo:2615 2024-11-06T04:54:02Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T04:54:02Z- handleDepth()- Depth_Keller msg: 1730868842.44 epoch sec; seqNo:2785; depth 50.51 2024-11-06T04:54:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868842.45 epoch sec; seqNo:2785; 4061730.37 north; 594870.72 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T04:54:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868838.84 2024-11-06T04:54:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:02Z- waiting for 1730868842.84, time is 1730868842.45 2024-11-06T04:54:02Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:54:02Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:54:02Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:54:02Z- AHRS_M2 msg: 1730868842.83 epoch sec; seqNo:2616 2024-11-06T04:54:02Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 1.50 psi 2024-11-06T04:54:02Z- handleDepth()- Depth_Keller msg: 1730868842.86 epoch sec; seqNo:2786; depth 50.51 2024-11-06T04:54:02Z- handleDVL() - DVL msg epochMillisec: 1730868842.88 2024-11-06T04:54:02Z- handleDvl() - RDI_Pathfinder msg: 1730868842.88 sec; seqNo:1019; #beams:4 2024-11-06T04:54:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.17 , 36.44 , 42.98 , 34.01 2024-11-06T04:54:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.71 , -0.06 , 0.07 2024-11-06T04:54:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868842.90 epoch sec; seqNo:2786; 4061730.37 north; 594870.72 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T04:54:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868838.84 2024-11-06T04:54:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:03Z- waiting for 1730868842.84, time is 1730868842.90 2024-11-06T04:54:03Z- LcmTrn::updateTrn() >>>> pose time 1730868842.90, meas time 1730868842.88 <<<< 2024-11-06T04:54:03Z- LcmTrn::updateTrn() equating pose 1730868842.88 and meas times 1730868842.88 2024-11-06T04:54:03Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868842.88 2024-11-06T04:54:03Z- TerrainNav::motionUpdate() - currEstimate = (4061730, 594870.72, 50.51) 2024-11-06T04:54:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:54:03Z- TerrainNav:: SubcloudNIS is 0.88, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:54:03Z- TNavPF::Projecting Measurements 2024-11-06T04:54:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:54:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:54:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:54:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:54:03Z- TNavFilter::4 good beams. 2024-11-06T04:54:03Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:54:03Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:54:03Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:54:03Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:54:03Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:54:03Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:54:03Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:54:03Z- TerrainMapOctree: (vehN, vehE ) = (4061730.37, 594870.72). 2024-11-06T04:54:03Z- TerrainMapOctree: Min Distance = 122.30. 2024-11-06T04:54:03Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:54:03Z- Forcing Subcloud Comparison 2024-11-06T04:54:03Z- Weighting particles with subcloud comparison 2024-11-06T04:54:03Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:03Z- meanExpectedMeasDiff: 3.196240 delta_rms_squared: 0.955490 alpha: 0.979497 2024-11-06T04:54:03Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:03Z- meanExpectedMeasDiff: 0.690765 delta_rms_squared: 0.682539 alpha: 0.971532 2024-11-06T04:54:03Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:03Z- meanExpectedMeasDiff: 4.139273 delta_rms_squared: 0.806993 alpha: 0.975816 2024-11-06T04:54:03Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:03Z- meanExpectedMeasDiff: -3.665900 delta_rms_squared: 0.807653 alpha: 0.975835 2024-11-06T04:54:03Z- Subcloud NIS: 0.882816 2024-11-06T04:54:03Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:54:03Z- TNavPF:: sumWeights = 0.007487 2024-11-06T04:54:03Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:54:03Z- TNavPF::First Term of Map Covariance Matrix is 0.06 2024-11-06T04:54:03Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:54:03Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868842.88 sec, ping # 0. 2024-11-06T04:54:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:54:03Z- variances([0], [2], and psi_berg) = 0.668628 0.219558 0.000000 2024-11-06T04:54:03Z- publishEstimatesDecoder() - at 1730868842.88 sec 2024-11-06T04:54:03Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:54:03Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061676.05 594874.03 62.73 2024-11-06T04:54:03Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061673.88 594875.30 63.12 lat/lon: 36.70 -121.94 2024-11-06T04:54:03Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.67 0.22 0.34 0.00 2024-11-06T04:54:03Z- AHRS_M2 msg: 1730868843.25 epoch sec; seqNo:2617 2024-11-06T04:54:03Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.49 psi 2024-11-06T04:54:03Z- handleDepth()- Depth_Keller msg: 1730868843.26 epoch sec; seqNo:2787; depth 50.53 2024-11-06T04:54:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868843.28 epoch sec; seqNo:2787; 4061730.37 north; 594870.72 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T04:54:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868842.88 2024-11-06T04:54:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:03Z- waiting for 1730868846.88, time is 1730868843.28 2024-11-06T04:54:03Z- AHRS_M2 msg: 1730868843.65 epoch sec; seqNo:2618 2024-11-06T04:54:03Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.49 psi 2024-11-06T04:54:03Z- handleDepth()- Depth_Keller msg: 1730868843.66 epoch sec; seqNo:2788; depth 50.52 2024-11-06T04:54:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868843.67 epoch sec; seqNo:2788; 4061730.75 north; 594870.71 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T04:54:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868842.88 2024-11-06T04:54:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:03Z- waiting for 1730868846.88, time is 1730868843.67 2024-11-06T04:54:04Z- AHRS_M2 msg: 1730868844.05 epoch sec; seqNo:2619 2024-11-06T04:54:04Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.49 psi 2024-11-06T04:54:04Z- handleDepth()- Depth_Keller msg: 1730868844.05 epoch sec; seqNo:2789; depth 50.54 2024-11-06T04:54:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868844.07 epoch sec; seqNo:2789; 4061730.76 north; 594871.93 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T04:54:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868842.88 2024-11-06T04:54:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:04Z- waiting for 1730868846.88, time is 1730868844.07 2024-11-06T04:54:04Z- AHRS_M2 msg: 1730868844.45 epoch sec; seqNo:2620 2024-11-06T04:54:04Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.49 psi 2024-11-06T04:54:04Z- handleDepth()- Depth_Keller msg: 1730868844.46 epoch sec; seqNo:2790; depth 50.55 2024-11-06T04:54:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868844.47 epoch sec; seqNo:2790; 4061730.76 north; 594871.93 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:54:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868842.88 2024-11-06T04:54:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:04Z- waiting for 1730868846.88, time is 1730868844.47 2024-11-06T04:54:04Z- AHRS_M2 msg: 1730868844.87 epoch sec; seqNo:2621 2024-11-06T04:54:04Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.48 psi 2024-11-06T04:54:04Z- handleDepth()- Depth_Keller msg: 1730868844.87 epoch sec; seqNo:2791; depth 50.55 2024-11-06T04:54:04Z- handleDVL() - DVL msg epochMillisec: 1730868844.89 2024-11-06T04:54:04Z- handleDvl() - RDI_Pathfinder msg: 1730868844.89 sec; seqNo:1020; #beams:4 2024-11-06T04:54:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.92 , 36.81 , 43.54 , 34.20 2024-11-06T04:54:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.13 2024-11-06T04:54:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868844.92 epoch sec; seqNo:2791; 4061730.76 north; 594871.93 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:54:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868842.88 2024-11-06T04:54:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:05Z- waiting for 1730868846.88, time is 1730868844.92 2024-11-06T04:54:05Z- AHRS_M2 msg: 1730868845.26 epoch sec; seqNo:2622 2024-11-06T04:54:05Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.48 psi 2024-11-06T04:54:05Z- handleDepth()- Depth_Keller msg: 1730868845.28 epoch sec; seqNo:2792; depth 50.56 2024-11-06T04:54:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868845.30 epoch sec; seqNo:2792; 4061730.78 north; 594873.15 east; 36.70 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T04:54:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868842.88 2024-11-06T04:54:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:05Z- waiting for 1730868846.88, time is 1730868845.30 2024-11-06T04:54:05Z- handleDepth()- Depth_Keller msg: 1730868845.68 epoch sec; seqNo:2793; depth 50.57 2024-11-06T04:54:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868845.70 epoch sec; seqNo:2793; 4061730.78 north; 594873.15 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T04:54:05Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:54:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868842.88 2024-11-06T04:54:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:05Z- waiting for 1730868846.88, time is 1730868845.70 2024-11-06T04:54:06Z- AHRS_M2 msg: 1730868846.07 epoch sec; seqNo:2623 2024-11-06T04:54:06Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:54:06Z- handleDepth()- Depth_Keller msg: 1730868846.07 epoch sec; seqNo:2794; depth 50.57 2024-11-06T04:54:06Z- handleDVL() - DVL msg epochMillisec: 1730868846.08 2024-11-06T04:54:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:54:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:54:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:54:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:54:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:54:06Z- handleDvl() - RDI_Pathfinder msg: 1730868846.08 sec; seqNo:1021; #beams:0 2024-11-06T04:54:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:54:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.03 , 0.13 2024-11-06T04:54:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868846.08 epoch sec; seqNo:2794; 4061730.78 north; 594873.15 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T04:54:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868842.88 2024-11-06T04:54:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:06Z- waiting for 1730868846.88, time is 1730868846.08 2024-11-06T04:54:06Z- handleDepth()- Depth_Keller msg: 1730868846.49 epoch sec; seqNo:2795; depth 50.56 2024-11-06T04:54:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868846.51 epoch sec; seqNo:2795; 4061730.78 north; 594873.15 east; 36.70 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T04:54:06Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:54:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868842.88 2024-11-06T04:54:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:06Z- waiting for 1730868846.88, time is 1730868846.51 2024-11-06T04:54:06Z- AHRS_M2 msg: 1730868846.88 epoch sec; seqNo:2624 2024-11-06T04:54:06Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:54:06Z- handleDepth()- Depth_Keller msg: 1730868846.88 epoch sec; seqNo:2796; depth 50.57 2024-11-06T04:54:06Z- handleDVL() - DVL msg epochMillisec: 1730868846.93 2024-11-06T04:54:06Z- handleDvl() - RDI_Pathfinder msg: 1730868846.93 sec; seqNo:1022; #beams:4 2024-11-06T04:54:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.29 , 37.56 , 44.10 , 34.38 2024-11-06T04:54:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , 0.09 2024-11-06T04:54:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868846.95 epoch sec; seqNo:2796; 4061731.17 north; 594874.37 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T04:54:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868842.88 2024-11-06T04:54:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:07Z- waiting for 1730868846.88, time is 1730868846.95 2024-11-06T04:54:07Z- LcmTrn::updateTrn() >>>> pose time 1730868846.95, meas time 1730868846.93 <<<< 2024-11-06T04:54:07Z- LcmTrn::updateTrn() equating pose 1730868846.93 and meas times 1730868846.93 2024-11-06T04:54:07Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868846.93 2024-11-06T04:54:07Z- TerrainNav::motionUpdate() - currEstimate = (4061731, 594874.37, 50.57) 2024-11-06T04:54:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:54:07Z- TerrainNav:: SubcloudNIS is 0.88, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:54:07Z- TNavPF::Projecting Measurements 2024-11-06T04:54:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:54:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:54:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:54:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:54:07Z- TNavFilter::4 good beams. 2024-11-06T04:54:07Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:54:07Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:54:07Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:54:07Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:54:07Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:54:07Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:54:07Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:54:07Z- TerrainMapOctree: (vehN, vehE ) = (4061731.17, 594874.37). 2024-11-06T04:54:07Z- TerrainMapOctree: Min Distance = 125.79. 2024-11-06T04:54:07Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:54:07Z- Forcing Subcloud Comparison 2024-11-06T04:54:07Z- Weighting particles with subcloud comparison 2024-11-06T04:54:07Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:07Z- meanExpectedMeasDiff: 4.290969 delta_rms_squared: 0.942289 alpha: 0.979216 2024-11-06T04:54:07Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:07Z- meanExpectedMeasDiff: 1.395177 delta_rms_squared: 0.695364 alpha: 0.972042 2024-11-06T04:54:07Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:07Z- meanExpectedMeasDiff: 4.873655 delta_rms_squared: 0.968038 alpha: 0.979758 2024-11-06T04:54:07Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:07Z- meanExpectedMeasDiff: -2.909231 delta_rms_squared: 0.786876 alpha: 0.975213 2024-11-06T04:54:07Z- Subcloud NIS: 0.927660 2024-11-06T04:54:07Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:54:07Z- TNavPF:: sumWeights = 0.004659 2024-11-06T04:54:07Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:54:07Z- TNavPF::First Term of Map Covariance Matrix is 0.06 2024-11-06T04:54:07Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:54:07Z- TerrainNav::Resampling particle filter... 2024-11-06T04:54:07Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868846.93 sec, ping # 0. 2024-11-06T04:54:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:54:07Z- variances([0], [2], and psi_berg) = 0.645592 0.208620 0.000000 2024-11-06T04:54:07Z- publishEstimatesDecoder() - at 1730868846.93 sec 2024-11-06T04:54:07Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:54:07Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061676.40 594878.53 63.34 2024-11-06T04:54:07Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061674.38 594879.00 62.63 lat/lon: 36.70 -121.94 2024-11-06T04:54:07Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.65 0.21 0.36 0.00 2024-11-06T04:54:07Z- AHRS_M2 msg: 1730868847.28 epoch sec; seqNo:2625 2024-11-06T04:54:07Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.48 psi 2024-11-06T04:54:07Z- handleDepth()- Depth_Keller msg: 1730868847.29 epoch sec; seqNo:2797; depth 50.55 2024-11-06T04:54:07Z- handleDVL() - DVL msg epochMillisec: 1730868847.29 2024-11-06T04:54:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:54:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:54:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:54:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:54:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:54:07Z- handleDvl() - RDI_Pathfinder msg: 1730868847.29 sec; seqNo:1023; #beams:0 2024-11-06T04:54:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:54:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , 0.01 , 0.09 2024-11-06T04:54:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868847.32 epoch sec; seqNo:2797; 4061731.17 north; 594874.37 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:54:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868846.93 2024-11-06T04:54:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:07Z- waiting for 1730868850.93, time is 1730868847.32 2024-11-06T04:54:07Z- handleDepth()- Depth_Keller msg: 1730868847.71 epoch sec; seqNo:2798; depth 50.55 2024-11-06T04:54:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868847.72 epoch sec; seqNo:2798; 4061731.17 north; 594874.37 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:54:07Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:54:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868846.93 2024-11-06T04:54:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:07Z- waiting for 1730868850.93, time is 1730868847.72 2024-11-06T04:54:08Z- AHRS_M2 msg: 1730868848.09 epoch sec; seqNo:2626 2024-11-06T04:54:08Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.49 psi 2024-11-06T04:54:08Z- handleDepth()- Depth_Keller msg: 1730868848.09 epoch sec; seqNo:2799; depth 50.55 2024-11-06T04:54:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868848.10 epoch sec; seqNo:2799; 4061731.18 north; 594875.59 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:54:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868846.93 2024-11-06T04:54:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:08Z- waiting for 1730868850.93, time is 1730868848.10 2024-11-06T04:54:08Z- AHRS_M2 msg: 1730868848.49 epoch sec; seqNo:2627 2024-11-06T04:54:08Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:54:08Z- handleDepth()- Depth_Keller msg: 1730868848.49 epoch sec; seqNo:2800; depth 50.54 2024-11-06T04:54:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868848.51 epoch sec; seqNo:2800; 4061731.18 north; 594875.59 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T04:54:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868846.93 2024-11-06T04:54:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:08Z- waiting for 1730868850.93, time is 1730868848.51 2024-11-06T04:54:08Z- AHRS_M2 msg: 1730868848.90 epoch sec; seqNo:2628 2024-11-06T04:54:08Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:54:08Z- handleDepth()- Depth_Keller msg: 1730868848.90 epoch sec; seqNo:2801; depth 50.51 2024-11-06T04:54:08Z- handleDVL() - DVL msg epochMillisec: 1730868848.92 2024-11-06T04:54:08Z- handleDvl() - RDI_Pathfinder msg: 1730868848.92 sec; seqNo:1024; #beams:4 2024-11-06T04:54:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.66 , 37.56 , 44.29 , 34.57 2024-11-06T04:54:08Z- handleDvl() - RDI_Pathfinder msg: velocities 1.01 , 0.03 , 0.00 2024-11-06T04:54:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868848.95 epoch sec; seqNo:2801; 4061731.18 north; 594875.59 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T04:54:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868846.93 2024-11-06T04:54:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:09Z- waiting for 1730868850.93, time is 1730868848.95 2024-11-06T04:54:09Z- AHRS_M2 msg: 1730868849.30 epoch sec; seqNo:2629 2024-11-06T04:54:09Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:54:09Z- handleDepth()- Depth_Keller msg: 1730868849.31 epoch sec; seqNo:2802; depth 50.50 2024-11-06T04:54:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868849.32 epoch sec; seqNo:2802; 4061731.19 north; 594876.81 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T04:54:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868846.93 2024-11-06T04:54:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:09Z- waiting for 1730868850.93, time is 1730868849.32 2024-11-06T04:54:09Z- AHRS_M2 msg: 1730868849.70 epoch sec; seqNo:2630 2024-11-06T04:54:09Z- AHRS_M2 msg: 0.16 phi; 0.03 theta; 1.50 psi 2024-11-06T04:54:09Z- handleDepth()- Depth_Keller msg: 1730868849.71 epoch sec; seqNo:2803; depth 50.48 2024-11-06T04:54:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868849.73 epoch sec; seqNo:2803; 4061731.19 north; 594876.81 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T04:54:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868846.93 2024-11-06T04:54:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:09Z- waiting for 1730868850.93, time is 1730868849.73 2024-11-06T04:54:10Z- AHRS_M2 msg: 1730868850.11 epoch sec; seqNo:2631 2024-11-06T04:54:10Z- AHRS_M2 msg: 0.16 phi; 0.03 theta; 1.51 psi 2024-11-06T04:54:10Z- handleDepth()- Depth_Keller msg: 1730868850.12 epoch sec; seqNo:2804; depth 50.45 2024-11-06T04:54:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868850.13 epoch sec; seqNo:2804; 4061731.19 north; 594876.81 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T04:54:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868846.93 2024-11-06T04:54:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:10Z- waiting for 1730868850.93, time is 1730868850.13 2024-11-06T04:54:10Z- AHRS_M2 msg: 1730868850.51 epoch sec; seqNo:2632 2024-11-06T04:54:10Z- AHRS_M2 msg: 0.16 phi; 0.03 theta; 1.51 psi 2024-11-06T04:54:10Z- handleDepth()- Depth_Keller msg: 1730868850.52 epoch sec; seqNo:2805; depth 50.43 2024-11-06T04:54:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868850.53 epoch sec; seqNo:2805; 4061731.21 north; 594878.03 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:54:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868846.93 2024-11-06T04:54:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:10Z- waiting for 1730868850.93, time is 1730868850.53 2024-11-06T04:54:10Z- AHRS_M2 msg: 1730868850.92 epoch sec; seqNo:2633 2024-11-06T04:54:10Z- AHRS_M2 msg: 0.16 phi; 0.03 theta; 1.51 psi 2024-11-06T04:54:10Z- handleDepth()- Depth_Keller msg: 1730868850.92 epoch sec; seqNo:2806; depth 50.41 2024-11-06T04:54:10Z- handleDVL() - DVL msg epochMillisec: 1730868850.95 2024-11-06T04:54:10Z- handleDvl() - RDI_Pathfinder msg: 1730868850.95 sec; seqNo:1025; #beams:4 2024-11-06T04:54:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.04 , 38.12 , 44.85 , 35.51 2024-11-06T04:54:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.02 , -0.09 2024-11-06T04:54:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868850.96 epoch sec; seqNo:2806; 4061731.21 north; 594878.03 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T04:54:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868846.93 2024-11-06T04:54:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:11Z- waiting for 1730868850.93, time is 1730868850.96 2024-11-06T04:54:11Z- LcmTrn::updateTrn() >>>> pose time 1730868850.96, meas time 1730868850.95 <<<< 2024-11-06T04:54:11Z- LcmTrn::updateTrn() equating pose 1730868850.95 and meas times 1730868850.95 2024-11-06T04:54:11Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868850.95 2024-11-06T04:54:11Z- TerrainNav::motionUpdate() - currEstimate = (4061731, 594878.03, 50.41) 2024-11-06T04:54:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:54:11Z- TerrainNav:: SubcloudNIS is 0.93, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:54:11Z- TNavPF::Projecting Measurements 2024-11-06T04:54:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:54:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:54:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:54:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:54:11Z- TNavFilter::4 good beams. 2024-11-06T04:54:11Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:54:11Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:54:11Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:54:11Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:54:11Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:54:11Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:54:11Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:54:11Z- TerrainMapOctree: (vehN, vehE ) = (4061731.21, 594878.03). 2024-11-06T04:54:11Z- TerrainMapOctree: Min Distance = 129.40. 2024-11-06T04:54:11Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:54:11Z- Forcing Subcloud Comparison 2024-11-06T04:54:11Z- Weighting particles with subcloud comparison 2024-11-06T04:54:11Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:11Z- meanExpectedMeasDiff: 3.355673 delta_rms_squared: 1.000679 alpha: 0.980405 2024-11-06T04:54:11Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:11Z- meanExpectedMeasDiff: 1.539992 delta_rms_squared: 0.680196 alpha: 0.971437 2024-11-06T04:54:11Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:11Z- meanExpectedMeasDiff: 4.108617 delta_rms_squared: 0.891603 alpha: 0.978061 2024-11-06T04:54:11Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:11Z- meanExpectedMeasDiff: -2.665282 delta_rms_squared: 0.787397 alpha: 0.975229 2024-11-06T04:54:11Z- Subcloud NIS: 0.920812 2024-11-06T04:54:11Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:54:11Z- TNavPF:: sumWeights = 0.020112 2024-11-06T04:54:11Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:54:11Z- TNavPF::First Term of Map Covariance Matrix is 0.16 2024-11-06T04:54:11Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:54:11Z- TerrainNav::Resampling particle filter... 2024-11-06T04:54:11Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868850.95 sec, ping # 0. 2024-11-06T04:54:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:54:11Z- variances([0], [2], and psi_berg) = 0.551927 0.187305 0.000000 2024-11-06T04:54:11Z- publishEstimatesDecoder() - at 1730868850.95 sec 2024-11-06T04:54:11Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:54:11Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061676.43 594882.16 62.19 2024-11-06T04:54:11Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061674.37 594882.69 61.98 lat/lon: 36.70 -121.94 2024-11-06T04:54:11Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.55 0.19 0.34 0.00 2024-11-06T04:54:11Z- AHRS_M2 msg: 1730868851.32 epoch sec; seqNo:2634 2024-11-06T04:54:11Z- AHRS_M2 msg: 0.16 phi; 0.03 theta; 1.51 psi 2024-11-06T04:54:11Z- handleDepth()- Depth_Keller msg: 1730868851.33 epoch sec; seqNo:2807; depth 50.38 2024-11-06T04:54:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868851.34 epoch sec; seqNo:2807; 4061731.21 north; 594878.03 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:54:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868850.95 2024-11-06T04:54:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:11Z- waiting for 1730868854.95, time is 1730868851.34 2024-11-06T04:54:11Z- AHRS_M2 msg: 1730868851.72 epoch sec; seqNo:2635 2024-11-06T04:54:11Z- AHRS_M2 msg: 0.16 phi; 0.03 theta; 1.51 psi 2024-11-06T04:54:11Z- handleDepth()- Depth_Keller msg: 1730868851.73 epoch sec; seqNo:2808; depth 50.36 2024-11-06T04:54:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868851.74 epoch sec; seqNo:2808; 4061731.22 north; 594879.25 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:54:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868850.95 2024-11-06T04:54:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:11Z- waiting for 1730868854.95, time is 1730868851.74 2024-11-06T04:54:12Z- AHRS_M2 msg: 1730868852.13 epoch sec; seqNo:2636 2024-11-06T04:54:12Z- AHRS_M2 msg: 0.16 phi; 0.03 theta; 1.51 psi 2024-11-06T04:54:12Z- handleDepth()- Depth_Keller msg: 1730868852.13 epoch sec; seqNo:2809; depth 50.32 2024-11-06T04:54:12Z- handleDVL() - DVL msg epochMillisec: 1730868852.14 2024-11-06T04:54:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:54:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:54:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:54:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:54:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:54:12Z- handleDvl() - RDI_Pathfinder msg: 1730868852.14 sec; seqNo:1026; #beams:0 2024-11-06T04:54:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:54:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , 0.02 , -0.09 2024-11-06T04:54:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868852.14 epoch sec; seqNo:2809; 4061731.22 north; 594879.25 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:54:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868850.95 2024-11-06T04:54:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:12Z- waiting for 1730868854.95, time is 1730868852.14 2024-11-06T04:54:12Z- AHRS_M2 msg: 1730868852.53 epoch sec; seqNo:2637 2024-11-06T04:54:12Z- AHRS_M2 msg: 0.16 phi; 0.03 theta; 1.51 psi 2024-11-06T04:54:12Z- handleDepth()- Depth_Keller msg: 1730868852.54 epoch sec; seqNo:2810; depth 50.28 2024-11-06T04:54:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868852.55 epoch sec; seqNo:2810; 4061731.22 north; 594879.25 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T04:54:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868850.95 2024-11-06T04:54:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:12Z- waiting for 1730868854.95, time is 1730868852.55 2024-11-06T04:54:12Z- AHRS_M2 msg: 1730868852.94 epoch sec; seqNo:2638 2024-11-06T04:54:12Z- AHRS_M2 msg: 0.16 phi; 0.03 theta; 1.51 psi 2024-11-06T04:54:12Z- handleDepth()- Depth_Keller msg: 1730868852.94 epoch sec; seqNo:2811; depth 50.26 2024-11-06T04:54:12Z- handleDVL() - DVL msg epochMillisec: 1730868852.97 2024-11-06T04:54:12Z- handleDvl() - RDI_Pathfinder msg: 1730868852.97 sec; seqNo:1027; #beams:4 2024-11-06T04:54:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.60 , 39.24 , 44.66 , 36.44 2024-11-06T04:54:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , -0.10 2024-11-06T04:54:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868852.98 epoch sec; seqNo:2811; 4061731.22 north; 594879.25 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:54:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868850.95 2024-11-06T04:54:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:13Z- waiting for 1730868854.95, time is 1730868852.98 2024-11-06T04:54:13Z- AHRS_M2 msg: 1730868853.34 epoch sec; seqNo:2639 2024-11-06T04:54:13Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.51 psi 2024-11-06T04:54:13Z- handleDepth()- Depth_Keller msg: 1730868853.35 epoch sec; seqNo:2812; depth 50.22 2024-11-06T04:54:13Z- handleDVL() - DVL msg epochMillisec: 1730868853.35 2024-11-06T04:54:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:54:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:54:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:54:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:54:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:54:13Z- handleDvl() - RDI_Pathfinder msg: 1730868853.35 sec; seqNo:1028; #beams:0 2024-11-06T04:54:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:54:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.00 , -0.10 2024-11-06T04:54:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868853.37 epoch sec; seqNo:2812; 4061731.24 north; 594880.47 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T04:54:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868850.95 2024-11-06T04:54:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:13Z- waiting for 1730868854.95, time is 1730868853.37 2024-11-06T04:54:13Z- AHRS_M2 msg: 1730868853.75 epoch sec; seqNo:2640 2024-11-06T04:54:13Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.51 psi 2024-11-06T04:54:13Z- handleDepth()- Depth_Keller msg: 1730868853.76 epoch sec; seqNo:2813; depth 50.20 2024-11-06T04:54:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868853.77 epoch sec; seqNo:2813; 4061731.24 north; 594880.47 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:54:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868850.95 2024-11-06T04:54:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:13Z- waiting for 1730868854.95, time is 1730868853.77 2024-11-06T04:54:14Z- AHRS_M2 msg: 1730868854.15 epoch sec; seqNo:2641 2024-11-06T04:54:14Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.51 psi 2024-11-06T04:54:14Z- handleDepth()- Depth_Keller msg: 1730868854.15 epoch sec; seqNo:2814; depth 50.16 2024-11-06T04:54:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868854.17 epoch sec; seqNo:2814; 4061731.24 north; 594880.47 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T04:54:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868850.95 2024-11-06T04:54:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:14Z- waiting for 1730868854.95, time is 1730868854.17 2024-11-06T04:54:14Z- AHRS_M2 msg: 1730868854.55 epoch sec; seqNo:2642 2024-11-06T04:54:14Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.51 psi 2024-11-06T04:54:14Z- handleDepth()- Depth_Keller msg: 1730868854.56 epoch sec; seqNo:2815; depth 50.14 2024-11-06T04:54:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868854.56 epoch sec; seqNo:2815; 4061731.25 north; 594881.69 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:54:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868850.95 2024-11-06T04:54:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:14Z- waiting for 1730868854.95, time is 1730868854.56 2024-11-06T04:54:14Z- AHRS_M2 msg: 1730868854.96 epoch sec; seqNo:2643 2024-11-06T04:54:14Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.51 psi 2024-11-06T04:54:14Z- handleDepth()- Depth_Keller msg: 1730868854.96 epoch sec; seqNo:2816; depth 50.12 2024-11-06T04:54:15Z- handleDVL() - DVL msg epochMillisec: 1730868854.99 2024-11-06T04:54:15Z- handleDvl() - RDI_Pathfinder msg: 1730868854.99 sec; seqNo:1029; #beams:4 2024-11-06T04:54:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.41 , 40.18 , 44.48 , 37.56 2024-11-06T04:54:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.05 , -0.03 2024-11-06T04:54:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868855.03 epoch sec; seqNo:2816; 4061731.63 north; 594881.69 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T04:54:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868850.95 2024-11-06T04:54:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:15Z- waiting for 1730868854.95, time is 1730868855.03 2024-11-06T04:54:15Z- LcmTrn::updateTrn() >>>> pose time 1730868855.03, meas time 1730868854.99 <<<< 2024-11-06T04:54:15Z- LcmTrn::updateTrn() equating pose 1730868854.99 and meas times 1730868854.99 2024-11-06T04:54:15Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868854.99 2024-11-06T04:54:15Z- TerrainNav::motionUpdate() - currEstimate = (4061732, 594881.69, 50.12) 2024-11-06T04:54:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:54:15Z- TerrainNav:: SubcloudNIS is 0.92, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:54:15Z- TNavPF::Projecting Measurements 2024-11-06T04:54:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:54:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:54:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:54:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:54:15Z- TNavFilter::4 good beams. 2024-11-06T04:54:15Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:54:15Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:54:15Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:54:15Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:54:15Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:54:15Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:54:15Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:54:15Z- TerrainMapOctree: (vehN, vehE ) = (4061731.63, 594881.69). 2024-11-06T04:54:15Z- TerrainMapOctree: Min Distance = 132.96. 2024-11-06T04:54:15Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:54:15Z- Forcing Subcloud Comparison 2024-11-06T04:54:15Z- Weighting particles with subcloud comparison 2024-11-06T04:54:15Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:15Z- meanExpectedMeasDiff: 3.363277 delta_rms_squared: 0.964430 alpha: 0.979684 2024-11-06T04:54:15Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:15Z- meanExpectedMeasDiff: 2.494801 delta_rms_squared: 0.685773 alpha: 0.971662 2024-11-06T04:54:15Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:15Z- meanExpectedMeasDiff: 2.934924 delta_rms_squared: 0.835879 alpha: 0.976632 2024-11-06T04:54:15Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:15Z- meanExpectedMeasDiff: -1.470307 delta_rms_squared: 0.781006 alpha: 0.975031 2024-11-06T04:54:15Z- Subcloud NIS: 0.921266 2024-11-06T04:54:15Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:54:15Z- TNavPF:: sumWeights = 0.068942 2024-11-06T04:54:15Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:54:15Z- TNavPF::First Term of Map Covariance Matrix is 0.44 2024-11-06T04:54:15Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.01 2024-11-06T04:54:15Z- TerrainNav::Resampling particle filter... 2024-11-06T04:54:15Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868854.99 sec, ping # 0. 2024-11-06T04:54:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:54:15Z- variances([0], [2], and psi_berg) = 0.552902 0.195712 0.000000 2024-11-06T04:54:15Z- publishEstimatesDecoder() - at 1730868854.99 sec 2024-11-06T04:54:15Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:54:15Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061676.85 594885.79 61.79 2024-11-06T04:54:15Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061674.82 594886.33 61.08 lat/lon: 36.70 -121.94 2024-11-06T04:54:15Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.55 0.20 0.35 0.00 2024-11-06T04:54:15Z- AHRS_M2 msg: 1730868855.36 epoch sec; seqNo:2644 2024-11-06T04:54:15Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.50 psi 2024-11-06T04:54:15Z- handleDepth()- Depth_Keller msg: 1730868855.37 epoch sec; seqNo:2817; depth 50.08 2024-11-06T04:54:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868855.38 epoch sec; seqNo:2817; 4061731.63 north; 594881.69 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T04:54:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868854.99 2024-11-06T04:54:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:15Z- waiting for 1730868858.99, time is 1730868855.38 2024-11-06T04:54:15Z- AHRS_M2 msg: 1730868855.76 epoch sec; seqNo:2645 2024-11-06T04:54:15Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.50 psi 2024-11-06T04:54:15Z- handleDepth()- Depth_Keller msg: 1730868855.77 epoch sec; seqNo:2818; depth 50.07 2024-11-06T04:54:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868855.79 epoch sec; seqNo:2818; 4061731.63 north; 594881.69 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T04:54:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868854.99 2024-11-06T04:54:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:15Z- waiting for 1730868858.99, time is 1730868855.79 2024-11-06T04:54:16Z- AHRS_M2 msg: 1730868856.17 epoch sec; seqNo:2646 2024-11-06T04:54:16Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.50 psi 2024-11-06T04:54:16Z- handleDepth()- Depth_Keller msg: 1730868856.17 epoch sec; seqNo:2819; depth 50.06 2024-11-06T04:54:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868856.18 epoch sec; seqNo:2819; 4061731.64 north; 594882.91 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T04:54:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868854.99 2024-11-06T04:54:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:16Z- waiting for 1730868858.99, time is 1730868856.18 2024-11-06T04:54:16Z- AHRS_M2 msg: 1730868856.58 epoch sec; seqNo:2647 2024-11-06T04:54:16Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.50 psi 2024-11-06T04:54:16Z- handleDepth()- Depth_Keller msg: 1730868856.59 epoch sec; seqNo:2820; depth 50.05 2024-11-06T04:54:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868856.59 epoch sec; seqNo:2820; 4061731.64 north; 594882.91 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T04:54:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868854.99 2024-11-06T04:54:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:16Z- waiting for 1730868858.99, time is 1730868856.59 2024-11-06T04:54:16Z- AHRS_M2 msg: 1730868856.98 epoch sec; seqNo:2648 2024-11-06T04:54:16Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:54:16Z- handleDepth()- Depth_Keller msg: 1730868856.98 epoch sec; seqNo:2821; depth 50.02 2024-11-06T04:54:17Z- handleDVL() - DVL msg epochMillisec: 1730868857.01 2024-11-06T04:54:17Z- handleDvl() - RDI_Pathfinder msg: 1730868857.01 sec; seqNo:1030; #beams:4 2024-11-06T04:54:17Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.05 , 40.57 , 44.24 , 37.92 2024-11-06T04:54:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.06 , 0.05 2024-11-06T04:54:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868857.02 epoch sec; seqNo:2821; 4061731.64 north; 594882.91 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:54:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868854.99 2024-11-06T04:54:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:17Z- waiting for 1730868858.99, time is 1730868857.02 2024-11-06T04:54:17Z- AHRS_M2 msg: 1730868857.38 epoch sec; seqNo:2649 2024-11-06T04:54:17Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:54:17Z- handleDepth()- Depth_Keller msg: 1730868857.38 epoch sec; seqNo:2822; depth 50.01 2024-11-06T04:54:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868857.41 epoch sec; seqNo:2822; 4061731.64 north; 594882.91 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:54:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868854.99 2024-11-06T04:54:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:17Z- waiting for 1730868858.99, time is 1730868857.41 2024-11-06T04:54:17Z- AHRS_M2 msg: 1730868857.78 epoch sec; seqNo:2650 2024-11-06T04:54:17Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:54:17Z- handleDepth()- Depth_Keller msg: 1730868857.79 epoch sec; seqNo:2823; depth 49.99 2024-11-06T04:54:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868857.81 epoch sec; seqNo:2823; 4061731.65 north; 594884.13 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T04:54:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868854.99 2024-11-06T04:54:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:17Z- waiting for 1730868858.99, time is 1730868857.81 2024-11-06T04:54:18Z- AHRS_M2 msg: 1730868858.19 epoch sec; seqNo:2651 2024-11-06T04:54:18Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:54:18Z- handleDepth()- Depth_Keller msg: 1730868858.19 epoch sec; seqNo:2824; depth 50.00 2024-11-06T04:54:18Z- handleDVL() - DVL msg epochMillisec: 1730868858.20 2024-11-06T04:54:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:54:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:54:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:54:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:54:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:54:18Z- handleDvl() - RDI_Pathfinder msg: 1730868858.20 sec; seqNo:1031; #beams:0 2024-11-06T04:54:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:54:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , -0.06 , 0.05 2024-11-06T04:54:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868858.20 epoch sec; seqNo:2824; 4061731.65 north; 594884.13 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T04:54:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868854.99 2024-11-06T04:54:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:18Z- waiting for 1730868858.99, time is 1730868858.20 2024-11-06T04:54:18Z- AHRS_M2 msg: 1730868858.59 epoch sec; seqNo:2652 2024-11-06T04:54:18Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:54:18Z- handleDepth()- Depth_Keller msg: 1730868858.62 epoch sec; seqNo:2825; depth 50.00 2024-11-06T04:54:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868858.63 epoch sec; seqNo:2825; 4061732.03 north; 594884.12 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T04:54:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868854.99 2024-11-06T04:54:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:18Z- waiting for 1730868858.99, time is 1730868858.63 2024-11-06T04:54:19Z- AHRS_M2 msg: 1730868858.99 epoch sec; seqNo:2653 2024-11-06T04:54:19Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.49 psi 2024-11-06T04:54:19Z- handleDepth()- Depth_Keller msg: 1730868859.00 epoch sec; seqNo:2826; depth 49.96 2024-11-06T04:54:19Z- handleDVL() - DVL msg epochMillisec: 1730868859.03 2024-11-06T04:54:19Z- handleDvl() - RDI_Pathfinder msg: 1730868859.03 sec; seqNo:1032; #beams:4 2024-11-06T04:54:19Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 40.57 , 44.44 , 38.53 2024-11-06T04:54:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.09 2024-11-06T04:54:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868859.04 epoch sec; seqNo:2826; 4061732.03 north; 594884.12 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:54:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868854.99 2024-11-06T04:54:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:19Z- waiting for 1730868858.99, time is 1730868859.04 2024-11-06T04:54:19Z- LcmTrn::updateTrn() >>>> pose time 1730868859.04, meas time 1730868859.03 <<<< 2024-11-06T04:54:19Z- LcmTrn::updateTrn() equating pose 1730868859.03 and meas times 1730868859.03 2024-11-06T04:54:19Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868859.03 2024-11-06T04:54:19Z- TerrainNav::motionUpdate() - currEstimate = (4061732, 594884.12, 49.96) 2024-11-06T04:54:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:54:19Z- TerrainNav:: SubcloudNIS is 0.92, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:54:19Z- TNavPF::Projecting Measurements 2024-11-06T04:54:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:54:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:54:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:54:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:54:19Z- TNavFilter::4 good beams. 2024-11-06T04:54:19Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:54:19Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:54:19Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:54:19Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:54:19Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:54:19Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:54:19Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:54:19Z- TerrainMapOctree: (vehN, vehE ) = (4061732.03, 594884.12). 2024-11-06T04:54:19Z- TerrainMapOctree: Min Distance = 135.32. 2024-11-06T04:54:19Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:54:19Z- Forcing Subcloud Comparison 2024-11-06T04:54:19Z- Weighting particles with subcloud comparison 2024-11-06T04:54:19Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:19Z- meanExpectedMeasDiff: 2.551111 delta_rms_squared: 0.970190 alpha: 0.979802 2024-11-06T04:54:19Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:19Z- meanExpectedMeasDiff: 2.107345 delta_rms_squared: 0.685198 alpha: 0.971639 2024-11-06T04:54:19Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:19Z- meanExpectedMeasDiff: 1.854764 delta_rms_squared: 0.842377 alpha: 0.976808 2024-11-06T04:54:19Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:19Z- meanExpectedMeasDiff: -1.280828 delta_rms_squared: 0.778349 alpha: 0.974948 2024-11-06T04:54:19Z- Subcloud NIS: 0.898095 2024-11-06T04:54:19Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:54:19Z- TNavPF:: sumWeights = 0.189321 2024-11-06T04:54:19Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:54:19Z- TNavPF::First Term of Map Covariance Matrix is 0.61 2024-11-06T04:54:19Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.02 2024-11-06T04:54:19Z- TerrainNav::Resampling particle filter... 2024-11-06T04:54:19Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868859.03 sec, ping # 0. 2024-11-06T04:54:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:54:19Z- variances([0], [2], and psi_berg) = 0.534778 0.192313 0.000000 2024-11-06T04:54:19Z- publishEstimatesDecoder() - at 1730868859.03 sec 2024-11-06T04:54:19Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:54:19Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061677.22 594888.23 61.49 2024-11-06T04:54:19Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061675.24 594888.76 60.50 lat/lon: 36.70 -121.94 2024-11-06T04:54:19Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.53 0.19 0.36 0.00 2024-11-06T04:54:19Z- AHRS_M2 msg: 1730868859.40 epoch sec; seqNo:2654 2024-11-06T04:54:19Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.49 psi 2024-11-06T04:54:19Z- handleDepth()- Depth_Keller msg: 1730868859.41 epoch sec; seqNo:2827; depth 49.97 2024-11-06T04:54:19Z- handleDVL() - DVL msg epochMillisec: 1730868859.42 2024-11-06T04:54:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:54:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:54:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:54:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:54:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:54:19Z- handleDvl() - RDI_Pathfinder msg: 1730868859.42 sec; seqNo:1033; #beams:0 2024-11-06T04:54:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:54:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.09 2024-11-06T04:54:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868859.43 epoch sec; seqNo:2827; 4061732.05 north; 594885.34 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T04:54:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868859.03 2024-11-06T04:54:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:19Z- waiting for 1730868863.03, time is 1730868859.43 2024-11-06T04:54:19Z- AHRS_M2 msg: 1730868859.81 epoch sec; seqNo:2655 2024-11-06T04:54:19Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.49 psi 2024-11-06T04:54:19Z- handleDepth()- Depth_Keller msg: 1730868859.82 epoch sec; seqNo:2828; depth 49.96 2024-11-06T04:54:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868859.83 epoch sec; seqNo:2828; 4061732.05 north; 594885.34 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:54:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868859.03 2024-11-06T04:54:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:19Z- waiting for 1730868863.03, time is 1730868859.83 2024-11-06T04:54:20Z- AHRS_M2 msg: 1730868860.21 epoch sec; seqNo:2656 2024-11-06T04:54:20Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.49 psi 2024-11-06T04:54:20Z- handleDepth()- Depth_Keller msg: 1730868860.22 epoch sec; seqNo:2829; depth 49.95 2024-11-06T04:54:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868860.23 epoch sec; seqNo:2829; 4061732.05 north; 594885.34 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T04:54:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868859.03 2024-11-06T04:54:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:20Z- waiting for 1730868863.03, time is 1730868860.23 2024-11-06T04:54:20Z- handleDepth()- Depth_Keller msg: 1730868860.64 epoch sec; seqNo:2830; depth 49.94 2024-11-06T04:54:20Z- handleDVL() - DVL msg epochMillisec: 1730868860.64 2024-11-06T04:54:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:54:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:54:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:54:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:54:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:54:20Z- handleDvl() - RDI_Pathfinder msg: 1730868860.64 sec; seqNo:1034; #beams:0 2024-11-06T04:54:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:54:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.05 , 0.09 2024-11-06T04:54:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868860.65 epoch sec; seqNo:2830; 4061732.05 north; 594885.34 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T04:54:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868859.03 2024-11-06T04:54:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:20Z- waiting for 1730868863.03, time is 1730868860.65 2024-11-06T04:54:21Z- AHRS_M2 msg: 1730868861.04 epoch sec; seqNo:2657 2024-11-06T04:54:21Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.49 psi 2024-11-06T04:54:21Z- handleDepth()- Depth_Keller msg: 1730868861.04 epoch sec; seqNo:2831; depth 49.93 2024-11-06T04:54:21Z- handleDVL() - DVL msg epochMillisec: 1730868861.08 2024-11-06T04:54:21Z- handleDvl() - RDI_Pathfinder msg: 1730868861.08 sec; seqNo:1035; #beams:4 2024-11-06T04:54:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.87 , 41.38 , 44.44 , 38.94 2024-11-06T04:54:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.05 2024-11-06T04:54:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868861.10 epoch sec; seqNo:2831; 4061732.06 north; 594886.56 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T04:54:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868859.03 2024-11-06T04:54:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:21Z- waiting for 1730868863.03, time is 1730868861.10 2024-11-06T04:54:21Z- AHRS_M2 msg: 1730868861.42 epoch sec; seqNo:2658 2024-11-06T04:54:21Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:54:21Z- handleDepth()- Depth_Keller msg: 1730868861.43 epoch sec; seqNo:2832; depth 49.93 2024-11-06T04:54:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868861.44 epoch sec; seqNo:2832; 4061732.06 north; 594886.56 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T04:54:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868859.03 2024-11-06T04:54:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:21Z- waiting for 1730868863.03, time is 1730868861.44 2024-11-06T04:54:21Z- AHRS_M2 msg: 1730868861.83 epoch sec; seqNo:2659 2024-11-06T04:54:21Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 1.50 psi 2024-11-06T04:54:21Z- handleDepth()- Depth_Keller msg: 1730868861.84 epoch sec; seqNo:2833; depth 49.92 2024-11-06T04:54:21Z- handleDVL() - DVL msg epochMillisec: 1730868861.84 2024-11-06T04:54:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:54:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:54:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:54:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:54:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:54:21Z- handleDvl() - RDI_Pathfinder msg: 1730868861.84 sec; seqNo:1036; #beams:0 2024-11-06T04:54:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:54:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , 0.05 2024-11-06T04:54:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868861.85 epoch sec; seqNo:2833; 4061732.44 north; 594886.56 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:54:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868859.03 2024-11-06T04:54:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:21Z- waiting for 1730868863.03, time is 1730868861.85 2024-11-06T04:54:22Z- AHRS_M2 msg: 1730868862.23 epoch sec; seqNo:2660 2024-11-06T04:54:22Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.50 psi 2024-11-06T04:54:22Z- handleDepth()- Depth_Keller msg: 1730868862.24 epoch sec; seqNo:2834; depth 49.89 2024-11-06T04:54:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868862.25 epoch sec; seqNo:2834; 4061732.45 north; 594887.78 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:54:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868859.03 2024-11-06T04:54:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:22Z- waiting for 1730868863.03, time is 1730868862.25 2024-11-06T04:54:22Z- AHRS_M2 msg: 1730868862.63 epoch sec; seqNo:2661 2024-11-06T04:54:22Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 1.50 psi 2024-11-06T04:54:22Z- handleDepth()- Depth_Keller msg: 1730868862.64 epoch sec; seqNo:2835; depth 49.88 2024-11-06T04:54:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868862.65 epoch sec; seqNo:2835; 4061732.45 north; 594887.78 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T04:54:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868859.03 2024-11-06T04:54:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:22Z- waiting for 1730868863.03, time is 1730868862.65 2024-11-06T04:54:23Z- AHRS_M2 msg: 1730868863.04 epoch sec; seqNo:2662 2024-11-06T04:54:23Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.50 psi 2024-11-06T04:54:23Z- handleDepth()- Depth_Keller msg: 1730868863.04 epoch sec; seqNo:2836; depth 49.88 2024-11-06T04:54:23Z- handleDVL() - DVL msg epochMillisec: 1730868863.07 2024-11-06T04:54:23Z- handleDvl() - RDI_Pathfinder msg: 1730868863.07 sec; seqNo:1037; #beams:4 2024-11-06T04:54:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.07 , 41.18 , 44.65 , 39.35 2024-11-06T04:54:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , -0.00 2024-11-06T04:54:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868863.09 epoch sec; seqNo:2836; 4061732.45 north; 594887.78 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T04:54:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868859.03 2024-11-06T04:54:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:23Z- waiting for 1730868863.03, time is 1730868863.09 2024-11-06T04:54:23Z- LcmTrn::updateTrn() >>>> pose time 1730868863.09, meas time 1730868863.07 <<<< 2024-11-06T04:54:23Z- LcmTrn::updateTrn() equating pose 1730868863.07 and meas times 1730868863.07 2024-11-06T04:54:23Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868863.07 2024-11-06T04:54:23Z- TerrainNav::motionUpdate() - currEstimate = (4061732, 594887.78, 49.88) 2024-11-06T04:54:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:54:23Z- TerrainNav:: SubcloudNIS is 0.90, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:54:23Z- TNavPF::Projecting Measurements 2024-11-06T04:54:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:54:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:54:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:54:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:54:23Z- TNavFilter::4 good beams. 2024-11-06T04:54:23Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:54:23Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:54:23Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:54:23Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:54:23Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:54:23Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:54:23Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:54:23Z- TerrainMapOctree: (vehN, vehE ) = (4061732.45, 594887.78). 2024-11-06T04:54:23Z- TerrainMapOctree: Min Distance = 138.89. 2024-11-06T04:54:23Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:54:23Z- Forcing Subcloud Comparison 2024-11-06T04:54:23Z- Weighting particles with subcloud comparison 2024-11-06T04:54:23Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:23Z- meanExpectedMeasDiff: 2.512638 delta_rms_squared: 0.971258 alpha: 0.979824 2024-11-06T04:54:23Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:23Z- meanExpectedMeasDiff: 2.072505 delta_rms_squared: 0.696091 alpha: 0.972071 2024-11-06T04:54:23Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:23Z- meanExpectedMeasDiff: 1.709991 delta_rms_squared: 0.841102 alpha: 0.976774 2024-11-06T04:54:23Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:23Z- meanExpectedMeasDiff: -1.222097 delta_rms_squared: 0.793470 alpha: 0.975414 2024-11-06T04:54:23Z- Subcloud NIS: 0.900077 2024-11-06T04:54:23Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:54:23Z- TNavPF:: sumWeights = 0.211524 2024-11-06T04:54:23Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:54:23Z- TNavPF::First Term of Map Covariance Matrix is 0.64 2024-11-06T04:54:23Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T04:54:23Z- TerrainNav::Resampling particle filter... 2024-11-06T04:54:23Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868863.07 sec, ping # 0. 2024-11-06T04:54:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:54:23Z- variances([0], [2], and psi_berg) = 0.526843 0.191042 0.000000 2024-11-06T04:54:23Z- publishEstimatesDecoder() - at 1730868863.07 sec 2024-11-06T04:54:23Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:54:23Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061677.82 594891.85 60.33 2024-11-06T04:54:23Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061675.68 594892.41 59.99 lat/lon: 36.70 -121.94 2024-11-06T04:54:23Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.53 0.19 0.35 0.00 2024-11-06T04:54:23Z- AHRS_M2 msg: 1730868863.44 epoch sec; seqNo:2663 2024-11-06T04:54:23Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.50 psi 2024-11-06T04:54:23Z- handleDepth()- Depth_Keller msg: 1730868863.45 epoch sec; seqNo:2837; depth 49.86 2024-11-06T04:54:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868863.47 epoch sec; seqNo:2837; 4061732.47 north; 594889.00 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:54:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868863.07 2024-11-06T04:54:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:23Z- waiting for 1730868867.07, time is 1730868863.47 2024-11-06T04:54:23Z- AHRS_M2 msg: 1730868863.84 epoch sec; seqNo:2664 2024-11-06T04:54:23Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 1.50 psi 2024-11-06T04:54:23Z- handleDepth()- Depth_Keller msg: 1730868863.86 epoch sec; seqNo:2838; depth 49.84 2024-11-06T04:54:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868863.87 epoch sec; seqNo:2838; 4061732.47 north; 594889.00 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T04:54:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868863.07 2024-11-06T04:54:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:24Z- waiting for 1730868867.07, time is 1730868863.87 2024-11-06T04:54:24Z- AHRS_M2 msg: 1730868864.25 epoch sec; seqNo:2665 2024-11-06T04:54:24Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.50 psi 2024-11-06T04:54:24Z- handleDepth()- Depth_Keller msg: 1730868864.26 epoch sec; seqNo:2839; depth 49.83 2024-11-06T04:54:24Z- handleDVL() - DVL msg epochMillisec: 1730868864.26 2024-11-06T04:54:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:54:24Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:54:24Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:54:24Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:54:24Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:54:24Z- handleDvl() - RDI_Pathfinder msg: 1730868864.26 sec; seqNo:1038; #beams:0 2024-11-06T04:54:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:54:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.01 , -0.00 2024-11-06T04:54:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868864.27 epoch sec; seqNo:2839; 4061732.47 north; 594889.00 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:54:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868863.07 2024-11-06T04:54:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:24Z- waiting for 1730868867.07, time is 1730868864.27 2024-11-06T04:54:24Z- AHRS_M2 msg: 1730868864.65 epoch sec; seqNo:2666 2024-11-06T04:54:24Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.50 psi 2024-11-06T04:54:24Z- handleDepth()- Depth_Keller msg: 1730868864.67 epoch sec; seqNo:2840; depth 49.82 2024-11-06T04:54:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868864.68 epoch sec; seqNo:2840; 4061732.47 north; 594889.00 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:54:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868863.07 2024-11-06T04:54:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:24Z- waiting for 1730868867.07, time is 1730868864.68 2024-11-06T04:54:25Z- AHRS_M2 msg: 1730868865.06 epoch sec; seqNo:2667 2024-11-06T04:54:25Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 1.50 psi 2024-11-06T04:54:25Z- handleDepth()- Depth_Keller msg: 1730868865.06 epoch sec; seqNo:2841; depth 49.79 2024-11-06T04:54:25Z- handleDVL() - DVL msg epochMillisec: 1730868865.10 2024-11-06T04:54:25Z- handleDvl() - RDI_Pathfinder msg: 1730868865.10 sec; seqNo:1039; #beams:4 2024-11-06T04:54:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 41.38 , 44.44 , 39.75 2024-11-06T04:54:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.03 2024-11-06T04:54:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868865.11 epoch sec; seqNo:2841; 4061732.48 north; 594890.22 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:54:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868863.07 2024-11-06T04:54:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:25Z- waiting for 1730868867.07, time is 1730868865.11 2024-11-06T04:54:25Z- AHRS_M2 msg: 1730868865.46 epoch sec; seqNo:2668 2024-11-06T04:54:25Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.50 psi 2024-11-06T04:54:25Z- handleDepth()- Depth_Keller msg: 1730868865.47 epoch sec; seqNo:2842; depth 49.78 2024-11-06T04:54:25Z- handleDVL() - DVL msg epochMillisec: 1730868865.48 2024-11-06T04:54:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:54:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:54:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:54:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:54:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:54:25Z- handleDvl() - RDI_Pathfinder msg: 1730868865.48 sec; seqNo:1040; #beams:0 2024-11-06T04:54:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:54:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.03 2024-11-06T04:54:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868865.49 epoch sec; seqNo:2842; 4061732.48 north; 594890.22 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:54:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868863.07 2024-11-06T04:54:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:25Z- waiting for 1730868867.07, time is 1730868865.49 2024-11-06T04:54:25Z- AHRS_M2 msg: 1730868865.86 epoch sec; seqNo:2669 2024-11-06T04:54:25Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 1.50 psi 2024-11-06T04:54:25Z- handleDepth()- Depth_Keller msg: 1730868865.87 epoch sec; seqNo:2843; depth 49.75 2024-11-06T04:54:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868865.88 epoch sec; seqNo:2843; 4061732.48 north; 594890.22 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:54:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868863.07 2024-11-06T04:54:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:26Z- waiting for 1730868867.07, time is 1730868865.88 2024-11-06T04:54:26Z- AHRS_M2 msg: 1730868866.27 epoch sec; seqNo:2670 2024-11-06T04:54:26Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T04:54:26Z- handleDepth()- Depth_Keller msg: 1730868866.28 epoch sec; seqNo:2844; depth 49.73 2024-11-06T04:54:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868866.29 epoch sec; seqNo:2844; 4061732.49 north; 594891.44 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T04:54:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868863.07 2024-11-06T04:54:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:26Z- waiting for 1730868867.07, time is 1730868866.29 2024-11-06T04:54:26Z- AHRS_M2 msg: 1730868866.68 epoch sec; seqNo:2671 2024-11-06T04:54:26Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T04:54:26Z- handleDepth()- Depth_Keller msg: 1730868866.70 epoch sec; seqNo:2845; depth 49.72 2024-11-06T04:54:26Z- handleDVL() - DVL msg epochMillisec: 1730868866.70 2024-11-06T04:54:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:54:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:54:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:54:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:54:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:54:26Z- handleDvl() - RDI_Pathfinder msg: 1730868866.70 sec; seqNo:1041; #beams:0 2024-11-06T04:54:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:54:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.03 2024-11-06T04:54:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868866.76 epoch sec; seqNo:2845; 4061732.49 north; 594891.44 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:54:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868863.07 2024-11-06T04:54:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:26Z- waiting for 1730868867.07, time is 1730868866.76 2024-11-06T04:54:27Z- AHRS_M2 msg: 1730868867.08 epoch sec; seqNo:2672 2024-11-06T04:54:27Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 1.50 psi 2024-11-06T04:54:27Z- handleDepth()- Depth_Keller msg: 1730868867.09 epoch sec; seqNo:2846; depth 49.71 2024-11-06T04:54:27Z- handleDVL() - DVL msg epochMillisec: 1730868867.12 2024-11-06T04:54:27Z- handleDvl() - RDI_Pathfinder msg: 1730868867.12 sec; seqNo:1042; #beams:4 2024-11-06T04:54:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.48 , 41.59 , 44.24 , 40.16 2024-11-06T04:54:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.00 , -0.02 2024-11-06T04:54:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868867.13 epoch sec; seqNo:2846; 4061732.49 north; 594891.44 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T04:54:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868863.07 2024-11-06T04:54:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:27Z- waiting for 1730868867.07, time is 1730868867.13 2024-11-06T04:54:27Z- LcmTrn::updateTrn() >>>> pose time 1730868867.13, meas time 1730868867.12 <<<< 2024-11-06T04:54:27Z- LcmTrn::updateTrn() equating pose 1730868867.12 and meas times 1730868867.12 2024-11-06T04:54:27Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868867.12 2024-11-06T04:54:27Z- TerrainNav::motionUpdate() - currEstimate = (4061732, 594891.44, 49.71) 2024-11-06T04:54:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:54:27Z- TerrainNav:: SubcloudNIS is 0.90, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:54:27Z- TNavPF::Projecting Measurements 2024-11-06T04:54:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:54:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:54:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:54:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:54:27Z- TNavFilter::4 good beams. 2024-11-06T04:54:27Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:54:27Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:54:27Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:54:27Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:54:27Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:54:27Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:54:27Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:54:27Z- TerrainMapOctree: (vehN, vehE ) = (4061732.49, 594891.44). 2024-11-06T04:54:27Z- TerrainMapOctree: Min Distance = 142.52. 2024-11-06T04:54:27Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:54:27Z- Forcing Subcloud Comparison 2024-11-06T04:54:27Z- Weighting particles with subcloud comparison 2024-11-06T04:54:27Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:27Z- meanExpectedMeasDiff: 2.303111 delta_rms_squared: 0.955082 alpha: 0.979489 2024-11-06T04:54:27Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:27Z- meanExpectedMeasDiff: 1.787003 delta_rms_squared: 0.695094 alpha: 0.972032 2024-11-06T04:54:27Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:27Z- meanExpectedMeasDiff: 1.106185 delta_rms_squared: 0.818043 alpha: 0.976135 2024-11-06T04:54:27Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:27Z- meanExpectedMeasDiff: -1.494558 delta_rms_squared: 0.805975 alpha: 0.975786 2024-11-06T04:54:27Z- Subcloud NIS: 0.905166 2024-11-06T04:54:27Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:54:27Z- TNavPF:: sumWeights = 0.257000 2024-11-06T04:54:27Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:54:27Z- TNavPF::First Term of Map Covariance Matrix is 0.67 2024-11-06T04:54:27Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T04:54:27Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730868867.12 sec, ping # 0. 2024-11-06T04:54:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:54:27Z- variances([0], [2], and psi_berg) = 0.545966 0.200132 0.000000 2024-11-06T04:54:27Z- publishEstimatesDecoder() - at 1730868867.12 sec 2024-11-06T04:54:27Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:54:27Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061673.20 594897.46 59.45 2024-11-06T04:54:27Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061675.72 594896.07 59.51 lat/lon: 36.70 -121.94 2024-11-06T04:54:27Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.55 0.20 0.36 0.00 2024-11-06T04:54:27Z- AHRS_M2 msg: 1730868867.48 epoch sec; seqNo:2673 2024-11-06T04:54:27Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 1.50 psi 2024-11-06T04:54:27Z- handleDepth()- Depth_Keller msg: 1730868867.49 epoch sec; seqNo:2847; depth 49.68 2024-11-06T04:54:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868867.50 epoch sec; seqNo:2847; 4061732.51 north; 594892.66 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T04:54:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868867.12 2024-11-06T04:54:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:27Z- waiting for 1730868871.12, time is 1730868867.50 2024-11-06T04:54:27Z- AHRS_M2 msg: 1730868867.88 epoch sec; seqNo:2674 2024-11-06T04:54:27Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.50 psi 2024-11-06T04:54:27Z- handleDepth()- Depth_Keller msg: 1730868867.89 epoch sec; seqNo:2848; depth 49.67 2024-11-06T04:54:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868867.90 epoch sec; seqNo:2848; 4061732.89 north; 594892.66 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:54:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868867.12 2024-11-06T04:54:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:28Z- waiting for 1730868871.12, time is 1730868867.90 2024-11-06T04:54:28Z- AHRS_M2 msg: 1730868868.29 epoch sec; seqNo:2675 2024-11-06T04:54:28Z- AHRS_M2 msg: 0.13 phi; 0.00 theta; 1.50 psi 2024-11-06T04:54:28Z- handleDepth()- Depth_Keller msg: 1730868868.30 epoch sec; seqNo:2849; depth 49.66 2024-11-06T04:54:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868868.31 epoch sec; seqNo:2849; 4061732.89 north; 594892.66 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:54:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868867.12 2024-11-06T04:54:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:28Z- waiting for 1730868871.12, time is 1730868868.31 2024-11-06T04:54:28Z- AHRS_M2 msg: 1730868868.73 epoch sec; seqNo:2676 2024-11-06T04:54:28Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.49 psi 2024-11-06T04:54:28Z- handleDepth()- Depth_Keller msg: 1730868868.74 epoch sec; seqNo:2850; depth 49.65 2024-11-06T04:54:28Z- handleDVL() - DVL msg epochMillisec: 1730868868.78 2024-11-06T04:54:28Z- handleDvl() - RDI_Pathfinder msg: 1730868868.78 sec; seqNo:1043; #beams:4 2024-11-06T04:54:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.07 , 41.79 , 44.24 , 40.37 2024-11-06T04:54:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.00 , -0.01 2024-11-06T04:54:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868868.79 epoch sec; seqNo:2850; 4061732.89 north; 594892.66 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:54:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868867.12 2024-11-06T04:54:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:28Z- waiting for 1730868871.12, time is 1730868868.79 2024-11-06T04:54:29Z- AHRS_M2 msg: 1730868869.10 epoch sec; seqNo:2677 2024-11-06T04:54:29Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.49 psi 2024-11-06T04:54:29Z- handleDepth()- Depth_Keller msg: 1730868869.11 epoch sec; seqNo:2851; depth 49.64 2024-11-06T04:54:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868869.12 epoch sec; seqNo:2851; 4061732.90 north; 594893.88 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:54:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868867.12 2024-11-06T04:54:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:29Z- waiting for 1730868871.12, time is 1730868869.12 2024-11-06T04:54:29Z- AHRS_M2 msg: 1730868869.50 epoch sec; seqNo:2678 2024-11-06T04:54:29Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.49 psi 2024-11-06T04:54:29Z- handleDepth()- Depth_Keller msg: 1730868869.50 epoch sec; seqNo:2852; depth 49.63 2024-11-06T04:54:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868869.52 epoch sec; seqNo:2852; 4061732.90 north; 594893.88 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:54:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868867.12 2024-11-06T04:54:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:29Z- waiting for 1730868871.12, time is 1730868869.52 2024-11-06T04:54:29Z- AHRS_M2 msg: 1730868869.90 epoch sec; seqNo:2679 2024-11-06T04:54:29Z- AHRS_M2 msg: 0.13 phi; -0.00 theta; 1.49 psi 2024-11-06T04:54:29Z- handleDepth()- Depth_Keller msg: 1730868869.91 epoch sec; seqNo:2853; depth 49.62 2024-11-06T04:54:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868869.92 epoch sec; seqNo:2853; 4061732.90 north; 594893.88 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:54:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868867.12 2024-11-06T04:54:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:30Z- waiting for 1730868871.12, time is 1730868869.92 2024-11-06T04:54:30Z- AHRS_M2 msg: 1730868870.31 epoch sec; seqNo:2680 2024-11-06T04:54:30Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.49 psi 2024-11-06T04:54:30Z- handleDepth()- Depth_Keller msg: 1730868870.31 epoch sec; seqNo:2854; depth 49.61 2024-11-06T04:54:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868870.32 epoch sec; seqNo:2854; 4061732.91 north; 594895.10 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:54:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868867.12 2024-11-06T04:54:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:30Z- waiting for 1730868871.12, time is 1730868870.32 2024-11-06T04:54:30Z- AHRS_M2 msg: 1730868870.74 epoch sec; seqNo:2681 2024-11-06T04:54:30Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:54:30Z- handleDepth()- Depth_Keller msg: 1730868870.75 epoch sec; seqNo:2855; depth 49.60 2024-11-06T04:54:30Z- handleDVL() - DVL msg epochMillisec: 1730868870.79 2024-11-06T04:54:30Z- handleDvl() - RDI_Pathfinder msg: 1730868870.79 sec; seqNo:1044; #beams:4 2024-11-06T04:54:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 41.79 , 44.03 , 40.57 2024-11-06T04:54:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , -0.01 2024-11-06T04:54:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868870.83 epoch sec; seqNo:2855; 4061732.91 north; 594895.10 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:54:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868867.12 2024-11-06T04:54:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:30Z- waiting for 1730868871.12, time is 1730868870.83 2024-11-06T04:54:31Z- AHRS_M2 msg: 1730868871.12 epoch sec; seqNo:2682 2024-11-06T04:54:31Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.49 psi 2024-11-06T04:54:31Z- handleDepth()- Depth_Keller msg: 1730868871.13 epoch sec; seqNo:2856; depth 49.59 2024-11-06T04:54:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868871.15 epoch sec; seqNo:2856; 4061732.91 north; 594895.10 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:54:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868867.12 2024-11-06T04:54:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:31Z- waiting for 1730868871.12, time is 1730868871.15 2024-11-06T04:54:31Z- LcmTrn::updateTrn() >>>> pose time 1730868871.15, meas time 1730868870.79 <<<< 2024-11-06T04:54:31Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868871.15 2024-11-06T04:54:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868870.79 sec, ping # 0; waiting for INS data more recent than 1730868867.12... 2024-11-06T04:54:31Z- TerrainNav::motionUpdate() - currEstimate = (4061733, 594895.10, 49.59) 2024-11-06T04:54:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:54:31Z- TerrainNav:: SubcloudNIS is 0.91, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:54:31Z- TNavPF::Projecting Measurements 2024-11-06T04:54:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:54:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:54:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:54:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:54:31Z- TNavFilter::4 good beams. 2024-11-06T04:54:31Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:54:31Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:54:31Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:54:31Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:54:31Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:54:31Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:54:31Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:54:31Z- TerrainMapOctree: (vehN, vehE ) = (4061732.87, 594894.77). 2024-11-06T04:54:31Z- TerrainMapOctree: Min Distance = 145.78. 2024-11-06T04:54:31Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:54:31Z- Forcing Subcloud Comparison 2024-11-06T04:54:31Z- Weighting particles with subcloud comparison 2024-11-06T04:54:31Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:31Z- meanExpectedMeasDiff: 1.586612 delta_rms_squared: 0.877474 alpha: 0.977715 2024-11-06T04:54:31Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:31Z- meanExpectedMeasDiff: 1.501119 delta_rms_squared: 0.706146 alpha: 0.972457 2024-11-06T04:54:31Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:31Z- meanExpectedMeasDiff: 0.755947 delta_rms_squared: 0.816270 alpha: 0.976084 2024-11-06T04:54:31Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:31Z- meanExpectedMeasDiff: -1.888624 delta_rms_squared: 0.817823 alpha: 0.976129 2024-11-06T04:54:31Z- Subcloud NIS: 0.876005 2024-11-06T04:54:31Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:54:31Z- TNavPF:: sumWeights = 0.310127 2024-11-06T04:54:31Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:54:31Z- TNavPF::First Term of Map Covariance Matrix is 0.47 2024-11-06T04:54:31Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T04:54:31Z- TerrainNav::Resampling particle filter... 2024-11-06T04:54:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868870.79 sec. 2024-11-06T04:54:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:54:31Z- variances([0], [2], and psi_berg) = 0.546788 0.195020 0.000000 2024-11-06T04:54:31Z- publishEstimatesDecoder() - at 1730868871.15 sec 2024-11-06T04:54:31Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:54:31Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061678.04 594899.23 59.58 2024-11-06T04:54:31Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061676.12 594899.73 59.24 lat/lon: 36.70 -121.94 2024-11-06T04:54:31Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.55 0.20 0.62 0.00 2024-11-06T04:54:31Z- AHRS_M2 msg: 1730868871.55 epoch sec; seqNo:2683 2024-11-06T04:54:31Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.49 psi 2024-11-06T04:54:31Z- handleDepth()- Depth_Keller msg: 1730868871.56 epoch sec; seqNo:2857; depth 49.58 2024-11-06T04:54:31Z- handleDVL() - DVL msg epochMillisec: 1730868871.56 2024-11-06T04:54:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:54:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:54:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:54:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:54:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:54:31Z- handleDvl() - RDI_Pathfinder msg: 1730868871.56 sec; seqNo:1045; #beams:0 2024-11-06T04:54:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:54:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.01 , -0.01 2024-11-06T04:54:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868871.58 epoch sec; seqNo:2857; 4061732.91 north; 594895.10 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:54:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868871.15 2024-11-06T04:54:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:31Z- waiting for 1730868875.15, time is 1730868871.58 2024-11-06T04:54:31Z- AHRS_M2 msg: 1730868871.93 epoch sec; seqNo:2684 2024-11-06T04:54:31Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:54:31Z- handleDepth()- Depth_Keller msg: 1730868871.93 epoch sec; seqNo:2858; depth 49.59 2024-11-06T04:54:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868871.94 epoch sec; seqNo:2858; 4061732.93 north; 594896.32 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:54:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868871.15 2024-11-06T04:54:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:32Z- waiting for 1730868875.15, time is 1730868871.94 2024-11-06T04:54:32Z- AHRS_M2 msg: 1730868872.33 epoch sec; seqNo:2685 2024-11-06T04:54:32Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:54:32Z- handleDepth()- Depth_Keller msg: 1730868872.34 epoch sec; seqNo:2859; depth 49.59 2024-11-06T04:54:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868872.35 epoch sec; seqNo:2859; 4061732.93 north; 594896.32 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:54:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868871.15 2024-11-06T04:54:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:32Z- waiting for 1730868875.15, time is 1730868872.35 2024-11-06T04:54:32Z- AHRS_M2 msg: 1730868872.76 epoch sec; seqNo:2686 2024-11-06T04:54:32Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:54:32Z- handleDepth()- Depth_Keller msg: 1730868872.76 epoch sec; seqNo:2860; depth 49.58 2024-11-06T04:54:32Z- handleDVL() - DVL msg epochMillisec: 1730868872.78 2024-11-06T04:54:32Z- handleDvl() - RDI_Pathfinder msg: 1730868872.78 sec; seqNo:1046; #beams:4 2024-11-06T04:54:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.48 , 41.79 , 43.83 , 40.77 2024-11-06T04:54:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.05 , 0.02 2024-11-06T04:54:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868872.80 epoch sec; seqNo:2860; 4061732.93 north; 594896.32 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:54:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868871.15 2024-11-06T04:54:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:32Z- waiting for 1730868875.15, time is 1730868872.80 2024-11-06T04:54:33Z- AHRS_M2 msg: 1730868873.13 epoch sec; seqNo:2687 2024-11-06T04:54:33Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:54:33Z- handleDepth()- Depth_Keller msg: 1730868873.14 epoch sec; seqNo:2861; depth 49.59 2024-11-06T04:54:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868873.16 epoch sec; seqNo:2861; 4061733.31 north; 594896.31 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:54:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868871.15 2024-11-06T04:54:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:33Z- waiting for 1730868875.15, time is 1730868873.16 2024-11-06T04:54:33Z- AHRS_M2 msg: 1730868873.54 epoch sec; seqNo:2688 2024-11-06T04:54:33Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:54:33Z- handleDepth()- Depth_Keller msg: 1730868873.54 epoch sec; seqNo:2862; depth 49.57 2024-11-06T04:54:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868873.55 epoch sec; seqNo:2862; 4061733.32 north; 594897.54 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T04:54:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868871.15 2024-11-06T04:54:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:33Z- waiting for 1730868875.15, time is 1730868873.55 2024-11-06T04:54:33Z- AHRS_M2 msg: 1730868873.94 epoch sec; seqNo:2689 2024-11-06T04:54:33Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:54:33Z- handleDepth()- Depth_Keller msg: 1730868873.95 epoch sec; seqNo:2863; depth 49.58 2024-11-06T04:54:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868873.96 epoch sec; seqNo:2863; 4061733.32 north; 594897.54 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:54:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868871.15 2024-11-06T04:54:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:34Z- waiting for 1730868875.15, time is 1730868873.96 2024-11-06T04:54:34Z- AHRS_M2 msg: 1730868874.35 epoch sec; seqNo:2690 2024-11-06T04:54:34Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.49 psi 2024-11-06T04:54:34Z- handleDepth()- Depth_Keller msg: 1730868874.36 epoch sec; seqNo:2864; depth 49.58 2024-11-06T04:54:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868874.37 epoch sec; seqNo:2864; 4061733.32 north; 594897.54 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:54:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868871.15 2024-11-06T04:54:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:34Z- waiting for 1730868875.15, time is 1730868874.37 2024-11-06T04:54:34Z- AHRS_M2 msg: 1730868874.76 epoch sec; seqNo:2691 2024-11-06T04:54:34Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.48 psi 2024-11-06T04:54:34Z- handleDepth()- Depth_Keller msg: 1730868874.77 epoch sec; seqNo:2865; depth 49.58 2024-11-06T04:54:34Z- handleDVL() - DVL msg epochMillisec: 1730868874.80 2024-11-06T04:54:34Z- handleDvl() - RDI_Pathfinder msg: 1730868874.80 sec; seqNo:1047; #beams:4 2024-11-06T04:54:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.48 , 42.00 , 43.63 , 41.38 2024-11-06T04:54:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.03 , 0.03 2024-11-06T04:54:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868874.82 epoch sec; seqNo:2865; 4061733.33 north; 594898.76 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:54:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868871.15 2024-11-06T04:54:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:34Z- waiting for 1730868875.15, time is 1730868874.82 2024-11-06T04:54:35Z- AHRS_M2 msg: 1730868875.16 epoch sec; seqNo:2692 2024-11-06T04:54:35Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 1.48 psi 2024-11-06T04:54:35Z- handleDepth()- Depth_Keller msg: 1730868875.17 epoch sec; seqNo:2866; depth 49.58 2024-11-06T04:54:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868875.18 epoch sec; seqNo:2866; 4061733.33 north; 594898.76 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:54:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868871.15 2024-11-06T04:54:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:35Z- waiting for 1730868875.15, time is 1730868875.18 2024-11-06T04:54:35Z- LcmTrn::updateTrn() >>>> pose time 1730868875.18, meas time 1730868874.80 <<<< 2024-11-06T04:54:35Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868875.18 2024-11-06T04:54:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868874.80 sec, ping # 0; waiting for INS data more recent than 1730868871.15... 2024-11-06T04:54:35Z- TerrainNav::motionUpdate() - currEstimate = (4061733, 594898.76, 49.58) 2024-11-06T04:54:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:54:35Z- TerrainNav:: SubcloudNIS is 0.88, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:54:35Z- TNavPF::Projecting Measurements 2024-11-06T04:54:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:54:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:54:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:54:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:54:35Z- TNavFilter::4 good beams. 2024-11-06T04:54:35Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:54:35Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:54:35Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:54:35Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:54:35Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:54:35Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:54:35Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:54:35Z- TerrainMapOctree: (vehN, vehE ) = (4061733.29, 594898.41). 2024-11-06T04:54:35Z- TerrainMapOctree: Min Distance = 149.35. 2024-11-06T04:54:35Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:54:35Z- Forcing Subcloud Comparison 2024-11-06T04:54:35Z- Weighting particles with subcloud comparison 2024-11-06T04:54:35Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:35Z- meanExpectedMeasDiff: 0.537626 delta_rms_squared: 1.195699 alpha: 0.983549 2024-11-06T04:54:35Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:35Z- meanExpectedMeasDiff: 1.546719 delta_rms_squared: 0.991964 alpha: 0.980236 2024-11-06T04:54:35Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:35Z- meanExpectedMeasDiff: 0.314106 delta_rms_squared: 1.138854 alpha: 0.982742 2024-11-06T04:54:35Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:35Z- meanExpectedMeasDiff: -2.297158 delta_rms_squared: 0.882241 alpha: 0.977833 2024-11-06T04:54:35Z- Subcloud NIS: 0.832268 2024-11-06T04:54:35Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:54:35Z- TNavPF:: sumWeights = 0.304835 2024-11-06T04:54:35Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:54:35Z- TNavPF::First Term of Map Covariance Matrix is 0.25 2024-11-06T04:54:35Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.05 2024-11-06T04:54:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868874.80 sec. 2024-11-06T04:54:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:54:35Z- variances([0], [2], and psi_berg) = 0.560821 0.201906 0.000000 2024-11-06T04:54:35Z- publishEstimatesDecoder() - at 1730868875.18 sec 2024-11-06T04:54:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:54:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061677.29 594903.36 59.11 2024-11-06T04:54:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061676.63 594903.35 59.18 lat/lon: 36.70 -121.94 2024-11-06T04:54:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.56 0.20 0.75 0.00 2024-11-06T04:54:35Z- handleDepth()- Depth_Keller msg: 1730868875.59 epoch sec; seqNo:2867; depth 49.57 2024-11-06T04:54:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868875.60 epoch sec; seqNo:2867; 4061733.33 north; 594898.76 east; 36.70 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T04:54:35Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:54:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868875.18 2024-11-06T04:54:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:35Z- waiting for 1730868879.18, time is 1730868875.60 2024-11-06T04:54:35Z- handleDepth()- Depth_Keller msg: 1730868875.98 epoch sec; seqNo:2868; depth 49.58 2024-11-06T04:54:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868876.00 epoch sec; seqNo:2868; 4061733.33 north; 594898.76 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:54:36Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:54:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868875.18 2024-11-06T04:54:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:36Z- waiting for 1730868879.18, time is 1730868876.00 2024-11-06T04:54:36Z- AHRS_M2 msg: 1730868876.37 epoch sec; seqNo:2693 2024-11-06T04:54:36Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.48 psi 2024-11-06T04:54:36Z- handleDepth()- Depth_Keller msg: 1730868876.37 epoch sec; seqNo:2869; depth 49.58 2024-11-06T04:54:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868876.38 epoch sec; seqNo:2869; 4061733.72 north; 594899.97 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:54:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868875.18 2024-11-06T04:54:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:36Z- waiting for 1730868879.18, time is 1730868876.38 2024-11-06T04:54:36Z- AHRS_M2 msg: 1730868876.77 epoch sec; seqNo:2694 2024-11-06T04:54:36Z- AHRS_M2 msg: 0.13 phi; -0.01 theta; 1.48 psi 2024-11-06T04:54:36Z- handleDepth()- Depth_Keller msg: 1730868876.78 epoch sec; seqNo:2870; depth 49.59 2024-11-06T04:54:36Z- handleDVL() - DVL msg epochMillisec: 1730868876.81 2024-11-06T04:54:36Z- handleDvl() - RDI_Pathfinder msg: 1730868876.81 sec; seqNo:1048; #beams:4 2024-11-06T04:54:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 42.00 , 43.63 , 41.38 2024-11-06T04:54:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.02 2024-11-06T04:54:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868876.84 epoch sec; seqNo:2870; 4061733.72 north; 594899.97 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:54:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868875.18 2024-11-06T04:54:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:36Z- waiting for 1730868879.18, time is 1730868876.84 2024-11-06T04:54:37Z- AHRS_M2 msg: 1730868877.18 epoch sec; seqNo:2695 2024-11-06T04:54:37Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.49 psi 2024-11-06T04:54:37Z- handleDepth()- Depth_Keller msg: 1730868877.19 epoch sec; seqNo:2871; depth 49.59 2024-11-06T04:54:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868877.20 epoch sec; seqNo:2871; 4061733.72 north; 594899.97 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:54:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868875.18 2024-11-06T04:54:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:37Z- waiting for 1730868879.18, time is 1730868877.20 2024-11-06T04:54:37Z- AHRS_M2 msg: 1730868877.58 epoch sec; seqNo:2696 2024-11-06T04:54:37Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.49 psi 2024-11-06T04:54:37Z- handleDepth()- Depth_Keller msg: 1730868877.59 epoch sec; seqNo:2872; depth 49.58 2024-11-06T04:54:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868877.60 epoch sec; seqNo:2872; 4061733.72 north; 594899.97 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:54:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868875.18 2024-11-06T04:54:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:37Z- waiting for 1730868879.18, time is 1730868877.60 2024-11-06T04:54:37Z- AHRS_M2 msg: 1730868877.98 epoch sec; seqNo:2697 2024-11-06T04:54:37Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.49 psi 2024-11-06T04:54:37Z- handleDepth()- Depth_Keller msg: 1730868877.99 epoch sec; seqNo:2873; depth 49.60 2024-11-06T04:54:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868878.00 epoch sec; seqNo:2873; 4061733.74 north; 594901.19 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:54:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868875.18 2024-11-06T04:54:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:38Z- waiting for 1730868879.18, time is 1730868878.00 2024-11-06T04:54:38Z- AHRS_M2 msg: 1730868878.39 epoch sec; seqNo:2698 2024-11-06T04:54:38Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.49 psi 2024-11-06T04:54:38Z- handleDepth()- Depth_Keller msg: 1730868878.39 epoch sec; seqNo:2874; depth 49.59 2024-11-06T04:54:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868878.42 epoch sec; seqNo:2874; 4061733.74 north; 594901.19 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:54:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868875.18 2024-11-06T04:54:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:38Z- waiting for 1730868879.18, time is 1730868878.42 2024-11-06T04:54:38Z- AHRS_M2 msg: 1730868878.79 epoch sec; seqNo:2699 2024-11-06T04:54:38Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.49 psi 2024-11-06T04:54:38Z- handleDepth()- Depth_Keller msg: 1730868878.80 epoch sec; seqNo:2875; depth 49.60 2024-11-06T04:54:38Z- handleDVL() - DVL msg epochMillisec: 1730868878.84 2024-11-06T04:54:38Z- handleDvl() - RDI_Pathfinder msg: 1730868878.84 sec; seqNo:1049; #beams:4 2024-11-06T04:54:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 42.40 , 43.63 , 41.59 2024-11-06T04:54:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , -0.01 2024-11-06T04:54:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868878.87 epoch sec; seqNo:2875; 4061733.74 north; 594901.19 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:54:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868875.18 2024-11-06T04:54:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:38Z- waiting for 1730868879.18, time is 1730868878.87 2024-11-06T04:54:39Z- AHRS_M2 msg: 1730868879.19 epoch sec; seqNo:2700 2024-11-06T04:54:39Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.49 psi 2024-11-06T04:54:39Z- handleDepth()- Depth_Keller msg: 1730868879.20 epoch sec; seqNo:2876; depth 49.60 2024-11-06T04:54:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868879.22 epoch sec; seqNo:2876; 4061733.75 north; 594902.41 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:54:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868875.18 2024-11-06T04:54:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:39Z- waiting for 1730868879.18, time is 1730868879.22 2024-11-06T04:54:39Z- LcmTrn::updateTrn() >>>> pose time 1730868879.22, meas time 1730868878.84 <<<< 2024-11-06T04:54:39Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868879.22 2024-11-06T04:54:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868878.84 sec, ping # 0; waiting for INS data more recent than 1730868875.18... 2024-11-06T04:54:39Z- TerrainNav::motionUpdate() - currEstimate = (4061734, 594902.41, 49.60) 2024-11-06T04:54:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:54:39Z- TerrainNav:: SubcloudNIS is 0.83, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:54:39Z- TNavPF::Projecting Measurements 2024-11-06T04:54:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:54:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:54:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:54:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:54:39Z- TNavFilter::4 good beams. 2024-11-06T04:54:39Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:54:39Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:54:39Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:54:39Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:54:39Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:54:39Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:54:39Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:54:39Z- TerrainMapOctree: (vehN, vehE ) = (4061733.71, 594902.07). 2024-11-06T04:54:39Z- TerrainMapOctree: Min Distance = 152.94. 2024-11-06T04:54:39Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:54:39Z- Forcing Subcloud Comparison 2024-11-06T04:54:39Z- Weighting particles with subcloud comparison 2024-11-06T04:54:39Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:39Z- meanExpectedMeasDiff: 0.229859 delta_rms_squared: 1.592524 alpha: 0.987597 2024-11-06T04:54:39Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:39Z- meanExpectedMeasDiff: 1.859366 delta_rms_squared: 1.167147 alpha: 0.983153 2024-11-06T04:54:39Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:39Z- meanExpectedMeasDiff: 0.451096 delta_rms_squared: 1.324894 alpha: 0.985129 2024-11-06T04:54:39Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:39Z- meanExpectedMeasDiff: -2.570912 delta_rms_squared: 1.380158 alpha: 0.985716 2024-11-06T04:54:39Z- Subcloud NIS: 0.782966 2024-11-06T04:54:39Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:54:39Z- TNavPF:: sumWeights = 0.222881 2024-11-06T04:54:39Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:54:39Z- TNavPF::First Term of Map Covariance Matrix is 0.18 2024-11-06T04:54:39Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T04:54:39Z- TerrainNav::Resampling particle filter... 2024-11-06T04:54:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868878.84 sec. 2024-11-06T04:54:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:54:39Z- variances([0], [2], and psi_berg) = 0.586413 0.221090 0.000000 2024-11-06T04:54:39Z- publishEstimatesDecoder() - at 1730868879.22 sec 2024-11-06T04:54:39Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:54:39Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061678.87 594906.65 58.85 2024-11-06T04:54:39Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061677.06 594907.00 59.24 lat/lon: 36.70 -121.94 2024-11-06T04:54:39Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.59 0.22 0.74 0.00 2024-11-06T04:54:39Z- AHRS_M2 msg: 1730868879.60 epoch sec; seqNo:2701 2024-11-06T04:54:39Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.49 psi 2024-11-06T04:54:39Z- handleDepth()- Depth_Keller msg: 1730868879.61 epoch sec; seqNo:2877; depth 49.60 2024-11-06T04:54:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868879.62 epoch sec; seqNo:2877; 4061733.75 north; 594902.41 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:54:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868879.22 2024-11-06T04:54:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:39Z- waiting for 1730868883.22, time is 1730868879.62 2024-11-06T04:54:40Z- AHRS_M2 msg: 1730868880.00 epoch sec; seqNo:2702 2024-11-06T04:54:40Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.49 psi 2024-11-06T04:54:40Z- handleDepth()- Depth_Keller msg: 1730868880.02 epoch sec; seqNo:2878; depth 49.60 2024-11-06T04:54:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868880.02 epoch sec; seqNo:2878; 4061733.75 north; 594902.41 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:54:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868879.22 2024-11-06T04:54:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:40Z- waiting for 1730868883.22, time is 1730868880.02 2024-11-06T04:54:40Z- AHRS_M2 msg: 1730868880.41 epoch sec; seqNo:2703 2024-11-06T04:54:40Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.49 psi 2024-11-06T04:54:40Z- handleDepth()- Depth_Keller msg: 1730868880.41 epoch sec; seqNo:2879; depth 49.59 2024-11-06T04:54:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868880.43 epoch sec; seqNo:2879; 4061734.14 north; 594903.63 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:54:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868879.22 2024-11-06T04:54:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:40Z- waiting for 1730868883.22, time is 1730868880.43 2024-11-06T04:54:40Z- AHRS_M2 msg: 1730868880.82 epoch sec; seqNo:2704 2024-11-06T04:54:40Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.49 psi 2024-11-06T04:54:40Z- handleDepth()- Depth_Keller msg: 1730868880.82 epoch sec; seqNo:2880; depth 49.58 2024-11-06T04:54:40Z- handleDVL() - DVL msg epochMillisec: 1730868880.86 2024-11-06T04:54:40Z- handleDvl() - RDI_Pathfinder msg: 1730868880.86 sec; seqNo:1050; #beams:4 2024-11-06T04:54:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 42.00 , 43.42 , 41.59 2024-11-06T04:54:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , -0.00 2024-11-06T04:54:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868880.87 epoch sec; seqNo:2880; 4061734.14 north; 594903.63 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:54:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868879.22 2024-11-06T04:54:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:40Z- waiting for 1730868883.22, time is 1730868880.87 2024-11-06T04:54:41Z- AHRS_M2 msg: 1730868881.22 epoch sec; seqNo:2705 2024-11-06T04:54:41Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.49 psi 2024-11-06T04:54:41Z- handleDepth()- Depth_Keller msg: 1730868881.23 epoch sec; seqNo:2881; depth 49.59 2024-11-06T04:54:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868881.25 epoch sec; seqNo:2881; 4061734.14 north; 594903.63 east; 36.70 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T04:54:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868879.22 2024-11-06T04:54:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:41Z- waiting for 1730868883.22, time is 1730868881.25 2024-11-06T04:54:41Z- AHRS_M2 msg: 1730868881.62 epoch sec; seqNo:2706 2024-11-06T04:54:41Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.49 psi 2024-11-06T04:54:41Z- handleDepth()- Depth_Keller msg: 1730868881.63 epoch sec; seqNo:2882; depth 49.61 2024-11-06T04:54:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868881.65 epoch sec; seqNo:2882; 4061734.14 north; 594903.63 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:54:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868879.22 2024-11-06T04:54:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:41Z- waiting for 1730868883.22, time is 1730868881.65 2024-11-06T04:54:42Z- AHRS_M2 msg: 1730868882.02 epoch sec; seqNo:2707 2024-11-06T04:54:42Z- AHRS_M2 msg: 0.13 phi; -0.02 theta; 1.49 psi 2024-11-06T04:54:42Z- handleDepth()- Depth_Keller msg: 1730868882.03 epoch sec; seqNo:2883; depth 49.60 2024-11-06T04:54:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868882.05 epoch sec; seqNo:2883; 4061734.16 north; 594904.85 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:54:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868879.22 2024-11-06T04:54:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:42Z- waiting for 1730868883.22, time is 1730868882.05 2024-11-06T04:54:42Z- AHRS_M2 msg: 1730868882.43 epoch sec; seqNo:2708 2024-11-06T04:54:42Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:54:42Z- handleDepth()- Depth_Keller msg: 1730868882.43 epoch sec; seqNo:2884; depth 49.61 2024-11-06T04:54:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868882.44 epoch sec; seqNo:2884; 4061734.16 north; 594904.85 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:54:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868879.22 2024-11-06T04:54:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:42Z- waiting for 1730868883.22, time is 1730868882.44 2024-11-06T04:54:42Z- AHRS_M2 msg: 1730868882.84 epoch sec; seqNo:2709 2024-11-06T04:54:42Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:54:42Z- handleDepth()- Depth_Keller msg: 1730868882.85 epoch sec; seqNo:2885; depth 49.60 2024-11-06T04:54:42Z- handleDVL() - DVL msg epochMillisec: 1730868882.87 2024-11-06T04:54:42Z- handleDvl() - RDI_Pathfinder msg: 1730868882.87 sec; seqNo:1051; #beams:4 2024-11-06T04:54:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.48 , 42.20 , 43.63 , 41.79 2024-11-06T04:54:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , 0.00 2024-11-06T04:54:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868882.88 epoch sec; seqNo:2885; 4061734.16 north; 594904.85 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:54:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868879.22 2024-11-06T04:54:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:42Z- waiting for 1730868883.22, time is 1730868882.88 2024-11-06T04:54:43Z- AHRS_M2 msg: 1730868883.23 epoch sec; seqNo:2710 2024-11-06T04:54:43Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.49 psi 2024-11-06T04:54:43Z- handleDepth()- Depth_Keller msg: 1730868883.24 epoch sec; seqNo:2886; depth 49.61 2024-11-06T04:54:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868883.27 epoch sec; seqNo:2886; 4061734.17 north; 594906.07 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:54:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868879.22 2024-11-06T04:54:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:43Z- waiting for 1730868883.22, time is 1730868883.27 2024-11-06T04:54:43Z- LcmTrn::updateTrn() >>>> pose time 1730868883.27, meas time 1730868882.87 <<<< 2024-11-06T04:54:43Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868883.27 2024-11-06T04:54:43Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868882.87 sec, ping # 0; waiting for INS data more recent than 1730868879.22... 2024-11-06T04:54:43Z- TerrainNav::motionUpdate() - currEstimate = (4061734, 594906.07, 49.61) 2024-11-06T04:54:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:54:43Z- TerrainNav:: SubcloudNIS is 0.78, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:54:43Z- TNavPF::Projecting Measurements 2024-11-06T04:54:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:54:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:54:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:54:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:54:43Z- TNavFilter::4 good beams. 2024-11-06T04:54:43Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:54:43Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:54:43Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:54:43Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:54:43Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:54:43Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:54:43Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:54:43Z- TerrainMapOctree: (vehN, vehE ) = (4061734.13, 594905.71). 2024-11-06T04:54:43Z- TerrainMapOctree: Min Distance = 156.51. 2024-11-06T04:54:43Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:54:43Z- Forcing Subcloud Comparison 2024-11-06T04:54:43Z- Weighting particles with subcloud comparison 2024-11-06T04:54:43Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:43Z- meanExpectedMeasDiff: 0.340669 delta_rms_squared: 1.611406 alpha: 0.987741 2024-11-06T04:54:43Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:43Z- meanExpectedMeasDiff: 1.760665 delta_rms_squared: 1.169834 alpha: 0.983191 2024-11-06T04:54:43Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:43Z- meanExpectedMeasDiff: 0.603750 delta_rms_squared: 1.325424 alpha: 0.985135 2024-11-06T04:54:43Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:43Z- meanExpectedMeasDiff: -2.493710 delta_rms_squared: 1.400694 alpha: 0.985922 2024-11-06T04:54:43Z- Subcloud NIS: 0.733499 2024-11-06T04:54:43Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:54:43Z- TNavPF:: sumWeights = 0.237814 2024-11-06T04:54:43Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:54:43Z- TNavPF::First Term of Map Covariance Matrix is 0.19 2024-11-06T04:54:43Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T04:54:43Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868882.87 sec. 2024-11-06T04:54:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:54:43Z- variances([0], [2], and psi_berg) = 0.594542 0.223349 0.000000 2024-11-06T04:54:43Z- publishEstimatesDecoder() - at 1730868883.27 sec 2024-11-06T04:54:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:54:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061680.25 594909.26 57.30 2024-11-06T04:54:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061677.48 594910.66 59.24 lat/lon: 36.70 -121.94 2024-11-06T04:54:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.59 0.22 0.71 0.00 2024-11-06T04:54:43Z- AHRS_M2 msg: 1730868883.64 epoch sec; seqNo:2711 2024-11-06T04:54:43Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:54:43Z- handleDepth()- Depth_Keller msg: 1730868883.65 epoch sec; seqNo:2887; depth 49.62 2024-11-06T04:54:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868883.66 epoch sec; seqNo:2887; 4061734.17 north; 594906.07 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:54:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868883.27 2024-11-06T04:54:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:43Z- waiting for 1730868887.27, time is 1730868883.66 2024-11-06T04:54:44Z- AHRS_M2 msg: 1730868884.05 epoch sec; seqNo:2712 2024-11-06T04:54:44Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.48 psi 2024-11-06T04:54:44Z- handleDepth()- Depth_Keller msg: 1730868884.06 epoch sec; seqNo:2888; depth 49.62 2024-11-06T04:54:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868884.07 epoch sec; seqNo:2888; 4061734.17 north; 594906.07 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:54:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868883.27 2024-11-06T04:54:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:44Z- waiting for 1730868887.27, time is 1730868884.07 2024-11-06T04:54:44Z- AHRS_M2 msg: 1730868884.45 epoch sec; seqNo:2713 2024-11-06T04:54:44Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.48 psi 2024-11-06T04:54:44Z- handleDepth()- Depth_Keller msg: 1730868884.45 epoch sec; seqNo:2889; depth 49.63 2024-11-06T04:54:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868884.47 epoch sec; seqNo:2889; 4061734.17 north; 594906.07 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:54:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868883.27 2024-11-06T04:54:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:44Z- waiting for 1730868887.27, time is 1730868884.47 2024-11-06T04:54:44Z- AHRS_M2 msg: 1730868884.85 epoch sec; seqNo:2714 2024-11-06T04:54:44Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.48 psi 2024-11-06T04:54:44Z- handleDepth()- Depth_Keller msg: 1730868884.86 epoch sec; seqNo:2890; depth 49.62 2024-11-06T04:54:44Z- handleDVL() - DVL msg epochMillisec: 1730868884.88 2024-11-06T04:54:44Z- handleDvl() - RDI_Pathfinder msg: 1730868884.88 sec; seqNo:1052; #beams:4 2024-11-06T04:54:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 42.40 , 43.42 , 41.79 2024-11-06T04:54:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , 0.01 2024-11-06T04:54:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868884.91 epoch sec; seqNo:2890; 4061734.18 north; 594907.29 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:54:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868883.27 2024-11-06T04:54:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:45Z- waiting for 1730868887.27, time is 1730868884.91 2024-11-06T04:54:45Z- AHRS_M2 msg: 1730868885.26 epoch sec; seqNo:2715 2024-11-06T04:54:45Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.48 psi 2024-11-06T04:54:45Z- handleDepth()- Depth_Keller msg: 1730868885.26 epoch sec; seqNo:2891; depth 49.62 2024-11-06T04:54:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868885.28 epoch sec; seqNo:2891; 4061734.56 north; 594907.29 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:54:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868883.27 2024-11-06T04:54:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:45Z- waiting for 1730868887.27, time is 1730868885.28 2024-11-06T04:54:45Z- AHRS_M2 msg: 1730868885.66 epoch sec; seqNo:2716 2024-11-06T04:54:45Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.48 psi 2024-11-06T04:54:45Z- handleDepth()- Depth_Keller msg: 1730868885.67 epoch sec; seqNo:2892; depth 49.63 2024-11-06T04:54:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868885.68 epoch sec; seqNo:2892; 4061734.56 north; 594907.29 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:54:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868883.27 2024-11-06T04:54:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:45Z- waiting for 1730868887.27, time is 1730868885.68 2024-11-06T04:54:46Z- AHRS_M2 msg: 1730868886.06 epoch sec; seqNo:2717 2024-11-06T04:54:46Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.48 psi 2024-11-06T04:54:46Z- handleDepth()- Depth_Keller msg: 1730868886.07 epoch sec; seqNo:2893; depth 49.63 2024-11-06T04:54:46Z- handleDVL() - DVL msg epochMillisec: 1730868886.07 2024-11-06T04:54:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:54:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:54:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:54:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:54:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:54:46Z- handleDvl() - RDI_Pathfinder msg: 1730868886.07 sec; seqNo:1053; #beams:0 2024-11-06T04:54:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:54:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , 0.01 2024-11-06T04:54:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868886.10 epoch sec; seqNo:2893; 4061734.58 north; 594908.51 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:54:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868883.27 2024-11-06T04:54:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:46Z- waiting for 1730868887.27, time is 1730868886.10 2024-11-06T04:54:46Z- AHRS_M2 msg: 1730868886.47 epoch sec; seqNo:2718 2024-11-06T04:54:46Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 1.48 psi 2024-11-06T04:54:46Z- handleDepth()- Depth_Keller msg: 1730868886.48 epoch sec; seqNo:2894; depth 49.63 2024-11-06T04:54:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868886.49 epoch sec; seqNo:2894; 4061734.58 north; 594908.51 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:54:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868883.27 2024-11-06T04:54:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:46Z- waiting for 1730868887.27, time is 1730868886.49 2024-11-06T04:54:46Z- AHRS_M2 msg: 1730868886.87 epoch sec; seqNo:2719 2024-11-06T04:54:46Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:54:46Z- handleDepth()- Depth_Keller msg: 1730868886.88 epoch sec; seqNo:2895; depth 49.63 2024-11-06T04:54:46Z- handleDVL() - DVL msg epochMillisec: 1730868886.90 2024-11-06T04:54:46Z- handleDvl() - RDI_Pathfinder msg: 1730868886.90 sec; seqNo:1054; #beams:4 2024-11-06T04:54:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.48 , 42.20 , 43.42 , 42.20 2024-11-06T04:54:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.00 2024-11-06T04:54:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868886.91 epoch sec; seqNo:2895; 4061734.58 north; 594908.51 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:54:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868883.27 2024-11-06T04:54:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:47Z- waiting for 1730868887.27, time is 1730868886.91 2024-11-06T04:54:47Z- AHRS_M2 msg: 1730868887.28 epoch sec; seqNo:2720 2024-11-06T04:54:47Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:54:47Z- handleDepth()- Depth_Keller msg: 1730868887.28 epoch sec; seqNo:2896; depth 49.63 2024-11-06T04:54:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868887.30 epoch sec; seqNo:2896; 4061734.58 north; 594908.51 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:54:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868883.27 2024-11-06T04:54:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:47Z- waiting for 1730868887.27, time is 1730868887.30 2024-11-06T04:54:47Z- LcmTrn::updateTrn() >>>> pose time 1730868887.30, meas time 1730868886.90 <<<< 2024-11-06T04:54:47Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868887.30 2024-11-06T04:54:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868886.90 sec, ping # 0; waiting for INS data more recent than 1730868883.27... 2024-11-06T04:54:47Z- TerrainNav::motionUpdate() - currEstimate = (4061735, 594908.51, 49.63) 2024-11-06T04:54:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:54:47Z- TerrainNav:: SubcloudNIS is 0.73, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:54:47Z- TNavPF::Projecting Measurements 2024-11-06T04:54:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:54:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:54:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:54:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:54:47Z- TNavFilter::4 good beams. 2024-11-06T04:54:47Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:54:47Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:54:47Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:54:47Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:54:47Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:54:47Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:54:47Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:54:47Z- TerrainMapOctree: (vehN, vehE ) = (4061734.54, 594908.26). 2024-11-06T04:54:47Z- TerrainMapOctree: Min Distance = 159.02. 2024-11-06T04:54:47Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:54:47Z- Forcing Subcloud Comparison 2024-11-06T04:54:47Z- Weighting particles with subcloud comparison 2024-11-06T04:54:47Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:47Z- meanExpectedMeasDiff: 0.351254 delta_rms_squared: 1.551570 alpha: 0.987274 2024-11-06T04:54:47Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:47Z- meanExpectedMeasDiff: 1.768902 delta_rms_squared: 1.126337 alpha: 0.982553 2024-11-06T04:54:47Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:47Z- meanExpectedMeasDiff: 0.453307 delta_rms_squared: 1.273514 alpha: 0.984538 2024-11-06T04:54:47Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:47Z- meanExpectedMeasDiff: -2.126263 delta_rms_squared: 1.355834 alpha: 0.985463 2024-11-06T04:54:47Z- Subcloud NIS: 0.662010 2024-11-06T04:54:47Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:54:47Z- TNavPF:: sumWeights = 0.293913 2024-11-06T04:54:47Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:54:47Z- TNavPF::First Term of Map Covariance Matrix is 0.23 2024-11-06T04:54:47Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T04:54:47Z- TerrainNav::Resampling particle filter... 2024-11-06T04:54:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868886.90 sec. 2024-11-06T04:54:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:54:47Z- variances([0], [2], and psi_berg) = 0.587581 0.222761 0.000000 2024-11-06T04:54:47Z- publishEstimatesDecoder() - at 1730868887.30 sec 2024-11-06T04:54:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:54:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061679.77 594912.79 59.11 2024-11-06T04:54:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061677.88 594913.11 59.23 lat/lon: 36.70 -121.94 2024-11-06T04:54:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.59 0.22 0.71 0.00 2024-11-06T04:54:47Z- AHRS_M2 msg: 1730868887.68 epoch sec; seqNo:2721 2024-11-06T04:54:47Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:54:47Z- handleDepth()- Depth_Keller msg: 1730868887.72 epoch sec; seqNo:2897; depth 49.63 2024-11-06T04:54:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868887.74 epoch sec; seqNo:2897; 4061734.59 north; 594909.73 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:54:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868887.30 2024-11-06T04:54:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:47Z- waiting for 1730868891.30, time is 1730868887.74 2024-11-06T04:54:48Z- AHRS_M2 msg: 1730868888.09 epoch sec; seqNo:2722 2024-11-06T04:54:48Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:54:48Z- handleDepth()- Depth_Keller msg: 1730868888.09 epoch sec; seqNo:2898; depth 49.62 2024-11-06T04:54:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868888.10 epoch sec; seqNo:2898; 4061734.59 north; 594909.73 east; 36.70 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T04:54:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868887.30 2024-11-06T04:54:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:48Z- waiting for 1730868891.30, time is 1730868888.10 2024-11-06T04:54:48Z- AHRS_M2 msg: 1730868888.49 epoch sec; seqNo:2723 2024-11-06T04:54:48Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:54:48Z- handleDepth()- Depth_Keller msg: 1730868888.49 epoch sec; seqNo:2899; depth 49.64 2024-11-06T04:54:48Z- handleDVL() - DVL msg epochMillisec: 1730868888.49 2024-11-06T04:54:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:54:48Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:54:48Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:54:48Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:54:48Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:54:48Z- handleDvl() - RDI_Pathfinder msg: 1730868888.49 sec; seqNo:1055; #beams:0 2024-11-06T04:54:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:54:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.03 , 0.00 2024-11-06T04:54:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868888.50 epoch sec; seqNo:2899; 4061734.59 north; 594909.73 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:54:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868887.30 2024-11-06T04:54:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:48Z- waiting for 1730868891.30, time is 1730868888.50 2024-11-06T04:54:48Z- AHRS_M2 msg: 1730868888.89 epoch sec; seqNo:2724 2024-11-06T04:54:48Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:54:48Z- handleDepth()- Depth_Keller msg: 1730868888.89 epoch sec; seqNo:2900; depth 49.64 2024-11-06T04:54:48Z- handleDVL() - DVL msg epochMillisec: 1730868888.91 2024-11-06T04:54:48Z- handleDvl() - RDI_Pathfinder msg: 1730868888.91 sec; seqNo:1056; #beams:4 2024-11-06T04:54:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.48 , 42.00 , 43.42 , 42.61 2024-11-06T04:54:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , -0.01 2024-11-06T04:54:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868888.94 epoch sec; seqNo:2900; 4061734.98 north; 594910.94 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:54:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868887.30 2024-11-06T04:54:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:49Z- waiting for 1730868891.30, time is 1730868888.94 2024-11-06T04:54:49Z- AHRS_M2 msg: 1730868889.30 epoch sec; seqNo:2725 2024-11-06T04:54:49Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:54:49Z- handleDepth()- Depth_Keller msg: 1730868889.31 epoch sec; seqNo:2901; depth 49.63 2024-11-06T04:54:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868889.32 epoch sec; seqNo:2901; 4061734.98 north; 594910.94 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:54:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868887.30 2024-11-06T04:54:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:49Z- waiting for 1730868891.30, time is 1730868889.32 2024-11-06T04:54:49Z- AHRS_M2 msg: 1730868889.70 epoch sec; seqNo:2726 2024-11-06T04:54:49Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:54:49Z- handleDepth()- Depth_Keller msg: 1730868889.71 epoch sec; seqNo:2902; depth 49.64 2024-11-06T04:54:49Z- handleDVL() - DVL msg epochMillisec: 1730868889.71 2024-11-06T04:54:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:54:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:54:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:54:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:54:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:54:49Z- handleDvl() - RDI_Pathfinder msg: 1730868889.71 sec; seqNo:1057; #beams:0 2024-11-06T04:54:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:54:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , -0.01 2024-11-06T04:54:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868889.72 epoch sec; seqNo:2902; 4061734.98 north; 594910.94 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:54:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868887.30 2024-11-06T04:54:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:49Z- waiting for 1730868891.30, time is 1730868889.72 2024-11-06T04:54:50Z- AHRS_M2 msg: 1730868890.10 epoch sec; seqNo:2727 2024-11-06T04:54:50Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 1.48 psi 2024-11-06T04:54:50Z- handleDepth()- Depth_Keller msg: 1730868890.11 epoch sec; seqNo:2903; depth 49.65 2024-11-06T04:54:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868890.12 epoch sec; seqNo:2903; 4061734.98 north; 594910.94 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:54:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868887.30 2024-11-06T04:54:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:50Z- waiting for 1730868891.30, time is 1730868890.12 2024-11-06T04:54:50Z- handleDepth()- Depth_Keller msg: 1730868890.53 epoch sec; seqNo:2904; depth 49.65 2024-11-06T04:54:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868890.54 epoch sec; seqNo:2904; 4061735.00 north; 594912.16 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:54:50Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:54:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868887.30 2024-11-06T04:54:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:50Z- waiting for 1730868891.30, time is 1730868890.54 2024-11-06T04:54:50Z- AHRS_M2 msg: 1730868890.91 epoch sec; seqNo:2728 2024-11-06T04:54:50Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 1.48 psi 2024-11-06T04:54:50Z- handleDepth()- Depth_Keller msg: 1730868890.92 epoch sec; seqNo:2905; depth 49.64 2024-11-06T04:54:50Z- handleDVL() - DVL msg epochMillisec: 1730868890.94 2024-11-06T04:54:50Z- handleDvl() - RDI_Pathfinder msg: 1730868890.94 sec; seqNo:1058; #beams:4 2024-11-06T04:54:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.69 , 42.00 , 43.63 , 43.02 2024-11-06T04:54:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.02 , -0.00 2024-11-06T04:54:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868890.96 epoch sec; seqNo:2905; 4061735.00 north; 594912.16 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:54:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868887.30 2024-11-06T04:54:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:51Z- waiting for 1730868891.30, time is 1730868890.96 2024-11-06T04:54:51Z- AHRS_M2 msg: 1730868891.32 epoch sec; seqNo:2729 2024-11-06T04:54:51Z- AHRS_M2 msg: 0.18 phi; -0.03 theta; 1.49 psi 2024-11-06T04:54:51Z- handleDepth()- Depth_Keller msg: 1730868891.33 epoch sec; seqNo:2906; depth 49.65 2024-11-06T04:54:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868891.34 epoch sec; seqNo:2906; 4061735.00 north; 594912.16 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:54:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868887.30 2024-11-06T04:54:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:51Z- waiting for 1730868891.30, time is 1730868891.34 2024-11-06T04:54:51Z- LcmTrn::updateTrn() >>>> pose time 1730868891.34, meas time 1730868890.94 <<<< 2024-11-06T04:54:51Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868891.34 2024-11-06T04:54:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868890.94 sec, ping # 0; waiting for INS data more recent than 1730868887.30... 2024-11-06T04:54:51Z- TerrainNav::motionUpdate() - currEstimate = (4061735, 594912.16, 49.65) 2024-11-06T04:54:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:54:51Z- TerrainNav:: SubcloudNIS is 0.66, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:54:51Z- TNavPF::Projecting Measurements 2024-11-06T04:54:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:54:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:54:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:54:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:54:51Z- TNavFilter::4 good beams. 2024-11-06T04:54:51Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:54:51Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:54:51Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:54:51Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:54:51Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:54:51Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:54:51Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:54:51Z- TerrainMapOctree: (vehN, vehE ) = (4061734.95, 594911.80). 2024-11-06T04:54:51Z- TerrainMapOctree: Min Distance = 162.50. 2024-11-06T04:54:51Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:54:51Z- Forcing Subcloud Comparison 2024-11-06T04:54:51Z- Weighting particles with subcloud comparison 2024-11-06T04:54:51Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:51Z- meanExpectedMeasDiff: -0.875786 delta_rms_squared: 1.626037 alpha: 0.987850 2024-11-06T04:54:51Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:51Z- meanExpectedMeasDiff: 2.134884 delta_rms_squared: 1.078615 alpha: 0.981795 2024-11-06T04:54:51Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:51Z- meanExpectedMeasDiff: 0.500419 delta_rms_squared: 1.264282 alpha: 0.984427 2024-11-06T04:54:51Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:51Z- meanExpectedMeasDiff: -1.366366 delta_rms_squared: 1.561689 alpha: 0.987355 2024-11-06T04:54:51Z- Subcloud NIS: 0.620072 2024-11-06T04:54:51Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:54:51Z- TNavPF:: sumWeights = 0.313078 2024-11-06T04:54:51Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:54:51Z- TNavPF::First Term of Map Covariance Matrix is 0.37 2024-11-06T04:54:51Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.04 2024-11-06T04:54:51Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868890.94 sec. 2024-11-06T04:54:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:54:51Z- variances([0], [2], and psi_berg) = 0.595573 0.218073 0.000000 2024-11-06T04:54:51Z- publishEstimatesDecoder() - at 1730868891.34 sec 2024-11-06T04:54:51Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:54:51Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061679.09 594916.88 58.35 2024-11-06T04:54:51Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061678.39 594916.77 59.21 lat/lon: 36.70 -121.94 2024-11-06T04:54:51Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.60 0.22 0.71 0.00 2024-11-06T04:54:51Z- AHRS_M2 msg: 1730868891.72 epoch sec; seqNo:2730 2024-11-06T04:54:51Z- AHRS_M2 msg: 0.20 phi; -0.03 theta; 1.50 psi 2024-11-06T04:54:51Z- handleDepth()- Depth_Keller msg: 1730868891.72 epoch sec; seqNo:2907; depth 49.66 2024-11-06T04:54:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868891.74 epoch sec; seqNo:2907; 4061735.00 north; 594912.16 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:54:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868891.34 2024-11-06T04:54:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:51Z- waiting for 1730868895.34, time is 1730868891.74 2024-11-06T04:54:52Z- AHRS_M2 msg: 1730868892.12 epoch sec; seqNo:2731 2024-11-06T04:54:52Z- AHRS_M2 msg: 0.23 phi; -0.04 theta; 1.52 psi 2024-11-06T04:54:52Z- handleDepth()- Depth_Keller msg: 1730868892.13 epoch sec; seqNo:2908; depth 49.67 2024-11-06T04:54:52Z- handleDVL() - DVL msg epochMillisec: 1730868892.13 2024-11-06T04:54:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:54:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:54:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:54:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:54:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:54:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:54:52Z- handleDvl() - RDI_Pathfinder msg: 1730868892.13 sec; seqNo:1059; #beams:0 2024-11-06T04:54:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:54:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.02 , -0.00 2024-11-06T04:54:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868892.14 epoch sec; seqNo:2908; 4061735.01 north; 594913.38 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:54:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868891.34 2024-11-06T04:54:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:52Z- waiting for 1730868895.34, time is 1730868892.14 2024-11-06T04:54:52Z- AHRS_M2 msg: 1730868892.53 epoch sec; seqNo:2732 2024-11-06T04:54:52Z- AHRS_M2 msg: 0.26 phi; -0.04 theta; 1.54 psi 2024-11-06T04:54:52Z- handleDepth()- Depth_Keller msg: 1730868892.54 epoch sec; seqNo:2909; depth 49.66 2024-11-06T04:54:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868892.54 epoch sec; seqNo:2909; 4061735.01 north; 594913.38 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:54:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868891.34 2024-11-06T04:54:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:52Z- waiting for 1730868895.34, time is 1730868892.54 2024-11-06T04:54:52Z- AHRS_M2 msg: 1730868892.93 epoch sec; seqNo:2733 2024-11-06T04:54:52Z- AHRS_M2 msg: 0.30 phi; -0.05 theta; 1.57 psi 2024-11-06T04:54:52Z- handleDepth()- Depth_Keller msg: 1730868892.94 epoch sec; seqNo:2910; depth 49.68 2024-11-06T04:54:52Z- handleDVL() - DVL msg epochMillisec: 1730868892.96 2024-11-06T04:54:52Z- handleDvl() - RDI_Pathfinder msg: 1730868892.96 sec; seqNo:1060; #beams:4 2024-11-06T04:54:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.95 , 40.57 , 44.03 , 42.61 2024-11-06T04:54:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.02 , 0.00 2024-11-06T04:54:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868892.98 epoch sec; seqNo:2910; 4061735.01 north; 594913.38 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T04:54:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868891.34 2024-11-06T04:54:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:53Z- waiting for 1730868895.34, time is 1730868892.98 2024-11-06T04:54:53Z- AHRS_M2 msg: 1730868893.34 epoch sec; seqNo:2734 2024-11-06T04:54:53Z- AHRS_M2 msg: 0.33 phi; -0.06 theta; 1.60 psi 2024-11-06T04:54:53Z- handleDepth()- Depth_Keller msg: 1730868893.35 epoch sec; seqNo:2911; depth 49.70 2024-11-06T04:54:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868893.37 epoch sec; seqNo:2911; 4061735.02 north; 594914.60 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T04:54:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868891.34 2024-11-06T04:54:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:53Z- waiting for 1730868895.34, time is 1730868893.37 2024-11-06T04:54:53Z- AHRS_M2 msg: 1730868893.74 epoch sec; seqNo:2735 2024-11-06T04:54:53Z- AHRS_M2 msg: 0.37 phi; -0.07 theta; 1.63 psi 2024-11-06T04:54:53Z- handleDepth()- Depth_Keller msg: 1730868893.74 epoch sec; seqNo:2912; depth 49.70 2024-11-06T04:54:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868893.76 epoch sec; seqNo:2912; 4061735.02 north; 594914.60 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T04:54:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868891.34 2024-11-06T04:54:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:53Z- waiting for 1730868895.34, time is 1730868893.76 2024-11-06T04:54:54Z- AHRS_M2 msg: 1730868894.15 epoch sec; seqNo:2736 2024-11-06T04:54:54Z- AHRS_M2 msg: 0.42 phi; -0.09 theta; 1.70 psi 2024-11-06T04:54:54Z- handleDepth()- Depth_Keller msg: 1730868894.16 epoch sec; seqNo:2913; depth 49.72 2024-11-06T04:54:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868894.17 epoch sec; seqNo:2913; 4061735.02 north; 594914.60 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:54:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868891.34 2024-11-06T04:54:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:54Z- waiting for 1730868895.34, time is 1730868894.17 2024-11-06T04:54:54Z- AHRS_M2 msg: 1730868894.55 epoch sec; seqNo:2737 2024-11-06T04:54:54Z- AHRS_M2 msg: 0.44 phi; -0.10 theta; 1.73 psi 2024-11-06T04:54:54Z- handleDepth()- Depth_Keller msg: 1730868894.56 epoch sec; seqNo:2914; depth 49.74 2024-11-06T04:54:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868894.57 epoch sec; seqNo:2914; 4061735.02 north; 594914.60 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T04:54:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868891.34 2024-11-06T04:54:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:54Z- waiting for 1730868895.34, time is 1730868894.57 2024-11-06T04:54:54Z- AHRS_M2 msg: 1730868894.95 epoch sec; seqNo:2738 2024-11-06T04:54:54Z- AHRS_M2 msg: 0.45 phi; -0.11 theta; 1.76 psi 2024-11-06T04:54:54Z- handleDepth()- Depth_Keller msg: 1730868894.96 epoch sec; seqNo:2915; depth 49.75 2024-11-06T04:54:54Z- handleDVL() - DVL msg epochMillisec: 1730868894.98 2024-11-06T04:54:55Z- handleDvl() - RDI_Pathfinder msg: 1730868894.98 sec; seqNo:1061; #beams:4 2024-11-06T04:54:55Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 58.92 , 38.94 , 46.07 , 47.71 2024-11-06T04:54:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.02 , 0.02 2024-11-06T04:54:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868895.00 epoch sec; seqNo:2915; 4061735.04 north; 594915.82 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:54:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868891.34 2024-11-06T04:54:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:55Z- waiting for 1730868895.34, time is 1730868895.00 2024-11-06T04:54:55Z- handleDepth()- Depth_Keller msg: 1730868895.38 epoch sec; seqNo:2916; depth 49.77 2024-11-06T04:54:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868895.39 epoch sec; seqNo:2916; 4061735.04 north; 594915.82 east; 36.70 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T04:54:55Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:54:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868891.34 2024-11-06T04:54:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:55Z- waiting for 1730868895.34, time is 1730868895.39 2024-11-06T04:54:55Z- LcmTrn::updateTrn() >>>> pose time 1730868895.39, meas time 1730868894.98 <<<< 2024-11-06T04:54:55Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868895.39 2024-11-06T04:54:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868894.98 sec, ping # 0; waiting for INS data more recent than 1730868891.34... 2024-11-06T04:54:55Z- TerrainNav::motionUpdate() - currEstimate = (4061735, 594915.82, 49.77) 2024-11-06T04:54:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:54:55Z- TerrainNav:: SubcloudNIS is 0.62, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:54:55Z- TNavPF::Projecting Measurements 2024-11-06T04:54:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:54:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:54:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:54:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:54:55Z- TNavFilter::4 good beams. 2024-11-06T04:54:55Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:54:55Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:54:55Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:54:55Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:54:55Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:54:55Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:54:55Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:54:55Z- TerrainMapOctree: (vehN, vehE ) = (4061735.03, 594915.45). 2024-11-06T04:54:55Z- TerrainMapOctree: Min Distance = 166.12. 2024-11-06T04:54:55Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:54:55Z- Forcing Subcloud Comparison 2024-11-06T04:54:55Z- Weighting particles with subcloud comparison 2024-11-06T04:54:55Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:55Z- meanExpectedMeasDiff: -3.444098 delta_rms_squared: 2.826544 alpha: 0.992974 2024-11-06T04:54:55Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:55Z- meanExpectedMeasDiff: 1.194087 delta_rms_squared: 0.974216 alpha: 0.979884 2024-11-06T04:54:55Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:55Z- meanExpectedMeasDiff: 1.391944 delta_rms_squared: 1.369312 alpha: 0.985604 2024-11-06T04:54:55Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:55Z- meanExpectedMeasDiff: -1.179360 delta_rms_squared: 0.998601 alpha: 0.980365 2024-11-06T04:54:55Z- Subcloud NIS: 0.609820 2024-11-06T04:54:55Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:54:55Z- TNavPF:: sumWeights = 0.196759 2024-11-06T04:54:55Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:54:55Z- TNavPF::First Term of Map Covariance Matrix is 1.52 2024-11-06T04:54:55Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.02 2024-11-06T04:54:55Z- TerrainNav::Resampling particle filter... 2024-11-06T04:54:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868894.98 sec. 2024-11-06T04:54:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:54:55Z- variances([0], [2], and psi_berg) = 0.593876 0.224513 0.000000 2024-11-06T04:54:55Z- publishEstimatesDecoder() - at 1730868895.39 sec 2024-11-06T04:54:55Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:54:55Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061680.16 594920.17 57.81 2024-11-06T04:54:55Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061678.43 594920.40 59.51 lat/lon: 36.70 -121.94 2024-11-06T04:54:55Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.59 0.22 0.75 0.00 2024-11-06T04:54:55Z- AHRS_M2 msg: 1730868895.78 epoch sec; seqNo:2739 2024-11-06T04:54:55Z- AHRS_M2 msg: 0.45 phi; -0.13 theta; 1.83 psi 2024-11-06T04:54:55Z- handleDepth()- Depth_Keller msg: 1730868895.78 epoch sec; seqNo:2917; depth 49.79 2024-11-06T04:54:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868895.79 epoch sec; seqNo:2917; 4061735.04 north; 594915.82 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:54:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868895.39 2024-11-06T04:54:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:55Z- waiting for 1730868899.39, time is 1730868895.79 2024-11-06T04:54:56Z- AHRS_M2 msg: 1730868896.17 epoch sec; seqNo:2740 2024-11-06T04:54:56Z- AHRS_M2 msg: 0.45 phi; -0.14 theta; 1.86 psi 2024-11-06T04:54:56Z- handleDepth()- Depth_Keller msg: 1730868896.17 epoch sec; seqNo:2918; depth 49.82 2024-11-06T04:54:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868896.18 epoch sec; seqNo:2918; 4061734.66 north; 594915.83 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:54:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868895.39 2024-11-06T04:54:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:56Z- waiting for 1730868899.39, time is 1730868896.18 2024-11-06T04:54:56Z- AHRS_M2 msg: 1730868896.59 epoch sec; seqNo:2741 2024-11-06T04:54:56Z- AHRS_M2 msg: 0.44 phi; -0.15 theta; 1.89 psi 2024-11-06T04:54:56Z- handleDepth()- Depth_Keller msg: 1730868896.59 epoch sec; seqNo:2919; depth 49.85 2024-11-06T04:54:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868896.60 epoch sec; seqNo:2919; 4061734.67 north; 594917.05 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:54:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868895.39 2024-11-06T04:54:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:56Z- waiting for 1730868899.39, time is 1730868896.60 2024-11-06T04:54:56Z- AHRS_M2 msg: 1730868896.98 epoch sec; seqNo:2742 2024-11-06T04:54:56Z- AHRS_M2 msg: 0.43 phi; -0.15 theta; 1.93 psi 2024-11-06T04:54:56Z- handleDepth()- Depth_Keller msg: 1730868896.99 epoch sec; seqNo:2920; depth 49.88 2024-11-06T04:54:57Z- handleDVL() - DVL msg epochMillisec: 1730868897.01 2024-11-06T04:54:57Z- handleDvl() - RDI_Pathfinder msg: 1730868897.01 sec; seqNo:1062; #beams:4 2024-11-06T04:54:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.61 , 38.65 , 45.94 , 50.80 2024-11-06T04:54:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , 0.01 , 0.03 2024-11-06T04:54:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868897.02 epoch sec; seqNo:2920; 4061734.67 north; 594917.05 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T04:54:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:54:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868895.39 2024-11-06T04:54:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:57Z- waiting for 1730868899.39, time is 1730868897.02 2024-11-06T04:54:57Z- AHRS_M2 msg: 1730868897.40 epoch sec; seqNo:2743 2024-11-06T04:54:57Z- AHRS_M2 msg: 0.42 phi; -0.16 theta; 1.96 psi 2024-11-06T04:54:57Z- handleDepth()- Depth_Keller msg: 1730868897.41 epoch sec; seqNo:2921; depth 49.91 2024-11-06T04:54:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868897.42 epoch sec; seqNo:2921; 4061734.67 north; 594917.05 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:54:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868895.39 2024-11-06T04:54:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:57Z- waiting for 1730868899.39, time is 1730868897.42 2024-11-06T04:54:57Z- AHRS_M2 msg: 1730868897.81 epoch sec; seqNo:2744 2024-11-06T04:54:57Z- AHRS_M2 msg: 0.41 phi; -0.16 theta; 2.00 psi 2024-11-06T04:54:57Z- handleDepth()- Depth_Keller msg: 1730868897.84 epoch sec; seqNo:2922; depth 49.95 2024-11-06T04:54:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868897.85 epoch sec; seqNo:2922; 4061734.29 north; 594917.05 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T04:54:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868895.39 2024-11-06T04:54:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:57Z- waiting for 1730868899.39, time is 1730868897.85 2024-11-06T04:54:58Z- handleDepth()- Depth_Keller msg: 1730868898.24 epoch sec; seqNo:2923; depth 50.00 2024-11-06T04:54:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868898.25 epoch sec; seqNo:2923; 4061734.31 north; 594918.27 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T04:54:58Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:54:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868895.39 2024-11-06T04:54:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:58Z- waiting for 1730868899.39, time is 1730868898.25 2024-11-06T04:54:58Z- AHRS_M2 msg: 1730868898.62 epoch sec; seqNo:2745 2024-11-06T04:54:58Z- AHRS_M2 msg: 0.40 phi; -0.17 theta; 2.07 psi 2024-11-06T04:54:58Z- handleDepth()- Depth_Keller msg: 1730868898.62 epoch sec; seqNo:2924; depth 50.05 2024-11-06T04:54:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868898.63 epoch sec; seqNo:2924; 4061733.93 north; 594918.28 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T04:54:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868895.39 2024-11-06T04:54:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:58Z- waiting for 1730868899.39, time is 1730868898.63 2024-11-06T04:54:59Z- handleDepth()- Depth_Keller msg: 1730868899.04 epoch sec; seqNo:2925; depth 50.09 2024-11-06T04:54:59Z- handleDVL() - DVL msg epochMillisec: 1730868899.06 2024-11-06T04:54:59Z- handleDvl() - RDI_Pathfinder msg: 1730868899.06 sec; seqNo:1063; #beams:4 2024-11-06T04:54:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 64.69 , 38.77 , 44.00 , 51.85 2024-11-06T04:54:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.00 , 0.02 2024-11-06T04:54:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868899.08 epoch sec; seqNo:2925; 4061733.93 north; 594918.28 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T04:54:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868895.39 2024-11-06T04:54:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:59Z- waiting for 1730868899.39, time is 1730868899.08 2024-11-06T04:54:59Z- AHRS_M2 msg: 1730868899.44 epoch sec; seqNo:2746 2024-11-06T04:54:59Z- AHRS_M2 msg: 0.40 phi; -0.17 theta; 2.14 psi 2024-11-06T04:54:59Z- handleDepth()- Depth_Keller msg: 1730868899.44 epoch sec; seqNo:2926; depth 50.15 2024-11-06T04:54:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868899.46 epoch sec; seqNo:2926; 4061733.93 north; 594918.28 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T04:54:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:54:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868895.39 2024-11-06T04:54:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:54:59Z- waiting for 1730868899.39, time is 1730868899.46 2024-11-06T04:54:59Z- LcmTrn::updateTrn() >>>> pose time 1730868899.46, meas time 1730868899.06 <<<< 2024-11-06T04:54:59Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868899.46 2024-11-06T04:54:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868899.06 sec, ping # 0; waiting for INS data more recent than 1730868895.39... 2024-11-06T04:54:59Z- TerrainNav::motionUpdate() - currEstimate = (4061734, 594918.28, 50.15) 2024-11-06T04:54:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:54:59Z- TerrainNav:: SubcloudNIS is 0.61, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:54:59Z- TNavPF::Projecting Measurements 2024-11-06T04:54:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:54:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:54:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:54:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:54:59Z- TNavFilter::4 good beams. 2024-11-06T04:54:59Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:54:59Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:54:59Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:54:59Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:54:59Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:54:59Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:54:59Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:54:59Z- TerrainMapOctree: (vehN, vehE ) = (4061734.04, 594918.03). 2024-11-06T04:54:59Z- TerrainMapOctree: Min Distance = 168.79. 2024-11-06T04:54:59Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:54:59Z- Forcing Subcloud Comparison 2024-11-06T04:54:59Z- Weighting particles with subcloud comparison 2024-11-06T04:54:59Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:59Z- meanExpectedMeasDiff: 2.267178 delta_rms_squared: 2.834544 alpha: 0.992994 2024-11-06T04:54:59Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:59Z- meanExpectedMeasDiff: 1.198396 delta_rms_squared: 1.020340 alpha: 0.980776 2024-11-06T04:54:59Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:59Z- meanExpectedMeasDiff: 1.466707 delta_rms_squared: 1.310574 alpha: 0.984969 2024-11-06T04:54:59Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:54:59Z- meanExpectedMeasDiff: 0.997725 delta_rms_squared: 1.935954 alpha: 0.989775 2024-11-06T04:54:59Z- Subcloud NIS: 0.602874 2024-11-06T04:54:59Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:54:59Z- TNavPF:: sumWeights = 0.462269 2024-11-06T04:54:59Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:54:59Z- TNavPF::First Term of Map Covariance Matrix is 0.83 2024-11-06T04:54:59Z- TNavPF::Calculated NIS Value : 0.09 numBeams normalized NIS: 0.02 2024-11-06T04:54:59Z- TerrainNav::Resampling particle filter... 2024-11-06T04:54:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868899.06 sec. 2024-11-06T04:54:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:54:59Z- variances([0], [2], and psi_berg) = 0.581088 0.219119 0.000000 2024-11-06T04:54:59Z- publishEstimatesDecoder() - at 1730868899.46 sec 2024-11-06T04:54:59Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:54:59Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061679.04 594922.65 57.84 2024-11-06T04:54:59Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061677.34 594922.85 59.30 lat/lon: 36.70 -121.94 2024-11-06T04:54:59Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.58 0.22 0.71 0.00 2024-11-06T04:54:59Z- AHRS_M2 msg: 1730868899.86 epoch sec; seqNo:2747 2024-11-06T04:54:59Z- AHRS_M2 msg: 0.40 phi; -0.17 theta; 2.17 psi 2024-11-06T04:54:59Z- handleDepth()- Depth_Keller msg: 1730868899.87 epoch sec; seqNo:2927; depth 50.20 2024-11-06T04:54:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868899.89 epoch sec; seqNo:2927; 4061733.56 north; 594919.50 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:55:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868899.46 2024-11-06T04:55:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:00Z- waiting for 1730868903.46, time is 1730868899.89 2024-11-06T04:55:00Z- AHRS_M2 msg: 1730868900.27 epoch sec; seqNo:2748 2024-11-06T04:55:00Z- AHRS_M2 msg: 0.40 phi; -0.17 theta; 2.21 psi 2024-11-06T04:55:00Z- handleDepth()- Depth_Keller msg: 1730868900.28 epoch sec; seqNo:2928; depth 50.25 2024-11-06T04:55:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868900.29 epoch sec; seqNo:2928; 4061733.56 north; 594919.50 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T04:55:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868899.46 2024-11-06T04:55:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:00Z- waiting for 1730868903.46, time is 1730868900.29 2024-11-06T04:55:00Z- handleDepth()- Depth_Keller msg: 1730868900.68 epoch sec; seqNo:2929; depth 50.29 2024-11-06T04:55:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868900.70 epoch sec; seqNo:2929; 4061733.18 north; 594919.51 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:55:00Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:55:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868899.46 2024-11-06T04:55:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:00Z- waiting for 1730868903.46, time is 1730868900.70 2024-11-06T04:55:01Z- AHRS_M2 msg: 1730868901.07 epoch sec; seqNo:2749 2024-11-06T04:55:01Z- AHRS_M2 msg: 0.40 phi; -0.16 theta; 2.28 psi 2024-11-06T04:55:01Z- handleDepth()- Depth_Keller msg: 1730868901.07 epoch sec; seqNo:2930; depth 50.35 2024-11-06T04:55:01Z- handleDVL() - DVL msg epochMillisec: 1730868901.09 2024-11-06T04:55:01Z- handleDvl() - RDI_Pathfinder msg: 1730868901.09 sec; seqNo:1064; #beams:4 2024-11-06T04:55:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 63.74 , 38.53 , 44.00 , 51.14 2024-11-06T04:55:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.81 , -0.01 , 0.00 2024-11-06T04:55:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868901.12 epoch sec; seqNo:2930; 4061733.18 north; 594919.51 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:55:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868899.46 2024-11-06T04:55:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:01Z- waiting for 1730868903.46, time is 1730868901.12 2024-11-06T04:55:01Z- AHRS_M2 msg: 1730868901.47 epoch sec; seqNo:2750 2024-11-06T04:55:01Z- AHRS_M2 msg: 0.40 phi; -0.16 theta; 2.32 psi 2024-11-06T04:55:01Z- handleDepth()- Depth_Keller msg: 1730868901.47 epoch sec; seqNo:2931; depth 50.40 2024-11-06T04:55:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868901.49 epoch sec; seqNo:2931; 4061732.80 north; 594919.51 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T04:55:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868899.46 2024-11-06T04:55:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:01Z- waiting for 1730868903.46, time is 1730868901.49 2024-11-06T04:55:01Z- AHRS_M2 msg: 1730868901.87 epoch sec; seqNo:2751 2024-11-06T04:55:01Z- AHRS_M2 msg: 0.40 phi; -0.15 theta; 2.36 psi 2024-11-06T04:55:01Z- handleDepth()- Depth_Keller msg: 1730868901.88 epoch sec; seqNo:2932; depth 50.43 2024-11-06T04:55:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868901.89 epoch sec; seqNo:2932; 4061732.44 north; 594920.73 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:55:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868899.46 2024-11-06T04:55:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:02Z- waiting for 1730868903.46, time is 1730868901.89 2024-11-06T04:55:02Z- AHRS_M2 msg: 1730868902.27 epoch sec; seqNo:2752 2024-11-06T04:55:02Z- AHRS_M2 msg: 0.40 phi; -0.15 theta; 2.40 psi 2024-11-06T04:55:02Z- handleDepth()- Depth_Keller msg: 1730868902.28 epoch sec; seqNo:2933; depth 50.48 2024-11-06T04:55:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868902.29 epoch sec; seqNo:2933; 4061732.44 north; 594920.73 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T04:55:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868899.46 2024-11-06T04:55:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:02Z- waiting for 1730868903.46, time is 1730868902.29 2024-11-06T04:55:02Z- AHRS_M2 msg: 1730868902.68 epoch sec; seqNo:2753 2024-11-06T04:55:02Z- AHRS_M2 msg: 0.40 phi; -0.15 theta; 2.43 psi 2024-11-06T04:55:02Z- handleDepth()- Depth_Keller msg: 1730868902.70 epoch sec; seqNo:2934; depth 50.52 2024-11-06T04:55:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868902.75 epoch sec; seqNo:2934; 4061732.06 north; 594920.74 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T04:55:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868899.46 2024-11-06T04:55:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:02Z- waiting for 1730868903.46, time is 1730868902.75 2024-11-06T04:55:03Z- AHRS_M2 msg: 1730868903.12 epoch sec; seqNo:2754 2024-11-06T04:55:03Z- AHRS_M2 msg: 0.39 phi; -0.14 theta; 2.47 psi 2024-11-06T04:55:03Z- handleDepth()- Depth_Keller msg: 1730868903.12 epoch sec; seqNo:2935; depth 50.57 2024-11-06T04:55:03Z- handleDVL() - DVL msg epochMillisec: 1730868903.16 2024-11-06T04:55:03Z- handleDvl() - RDI_Pathfinder msg: 1730868903.16 sec; seqNo:1065; #beams:4 2024-11-06T04:55:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.79 , 38.06 , 44.48 , 50.66 2024-11-06T04:55:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , -0.02 2024-11-06T04:55:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868903.17 epoch sec; seqNo:2935; 4061731.68 north; 594920.74 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T04:55:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868899.46 2024-11-06T04:55:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:03Z- waiting for 1730868903.46, time is 1730868903.17 2024-11-06T04:55:03Z- AHRS_M2 msg: 1730868903.52 epoch sec; seqNo:2755 2024-11-06T04:55:03Z- AHRS_M2 msg: 0.39 phi; -0.14 theta; 2.51 psi 2024-11-06T04:55:03Z- handleDepth()- Depth_Keller msg: 1730868903.53 epoch sec; seqNo:2936; depth 50.62 2024-11-06T04:55:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868903.54 epoch sec; seqNo:2936; 4061731.68 north; 594920.74 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T04:55:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868899.46 2024-11-06T04:55:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:03Z- waiting for 1730868903.46, time is 1730868903.54 2024-11-06T04:55:03Z- LcmTrn::updateTrn() >>>> pose time 1730868903.54, meas time 1730868903.16 <<<< 2024-11-06T04:55:03Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868903.54 2024-11-06T04:55:03Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868903.16 sec, ping # 0; waiting for INS data more recent than 1730868899.46... 2024-11-06T04:55:03Z- TerrainNav::motionUpdate() - currEstimate = (4061732, 594920.74, 50.62) 2024-11-06T04:55:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:55:03Z- TerrainNav:: SubcloudNIS is 0.60, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:55:03Z- TNavPF::Projecting Measurements 2024-11-06T04:55:03Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:55:03Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:55:03Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:55:03Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:55:03Z- TNavFilter::4 good beams. 2024-11-06T04:55:03Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:55:03Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:55:03Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:55:03Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:55:03Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:55:03Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:55:03Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:55:03Z- TerrainMapOctree: (vehN, vehE ) = (4061731.89, 594920.51). 2024-11-06T04:55:03Z- TerrainMapOctree: Min Distance = 171.47. 2024-11-06T04:55:03Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:55:03Z- Forcing Subcloud Comparison 2024-11-06T04:55:03Z- Weighting particles with subcloud comparison 2024-11-06T04:55:03Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:03Z- meanExpectedMeasDiff: 1.459643 delta_rms_squared: 2.602829 alpha: 0.992375 2024-11-06T04:55:03Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:03Z- meanExpectedMeasDiff: 0.426166 delta_rms_squared: 0.973530 alpha: 0.979870 2024-11-06T04:55:03Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:03Z- meanExpectedMeasDiff: 1.270124 delta_rms_squared: 1.592019 alpha: 0.987593 2024-11-06T04:55:03Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:03Z- meanExpectedMeasDiff: 0.945866 delta_rms_squared: 1.803649 alpha: 0.989033 2024-11-06T04:55:03Z- Subcloud NIS: 0.573767 2024-11-06T04:55:03Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:55:03Z- TNavPF:: sumWeights = 0.552005 2024-11-06T04:55:03Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:55:03Z- TNavPF::First Term of Map Covariance Matrix is 0.75 2024-11-06T04:55:03Z- TNavPF::Calculated NIS Value : 0.07 numBeams normalized NIS: 0.02 2024-11-06T04:55:03Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868903.16 sec. 2024-11-06T04:55:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:55:03Z- variances([0], [2], and psi_berg) = 0.566787 0.210579 0.000000 2024-11-06T04:55:03Z- publishEstimatesDecoder() - at 1730868903.54 sec 2024-11-06T04:55:03Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:55:03Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061675.85 594925.96 58.89 2024-11-06T04:55:03Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061675.12 594925.32 59.37 lat/lon: 36.70 -121.94 2024-11-06T04:55:03Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.21 0.69 0.00 2024-11-06T04:55:03Z- AHRS_M2 msg: 1730868903.93 epoch sec; seqNo:2756 2024-11-06T04:55:03Z- AHRS_M2 msg: 0.39 phi; -0.13 theta; 2.55 psi 2024-11-06T04:55:03Z- handleDepth()- Depth_Keller msg: 1730868903.95 epoch sec; seqNo:2937; depth 50.67 2024-11-06T04:55:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868904.00 epoch sec; seqNo:2937; 4061731.30 north; 594920.75 east; 36.70 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T04:55:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868903.54 2024-11-06T04:55:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:04Z- waiting for 1730868907.54, time is 1730868904.00 2024-11-06T04:55:04Z- AHRS_M2 msg: 1730868904.33 epoch sec; seqNo:2757 2024-11-06T04:55:04Z- AHRS_M2 msg: 0.39 phi; -0.12 theta; 2.58 psi 2024-11-06T04:55:04Z- handleDepth()- Depth_Keller msg: 1730868904.35 epoch sec; seqNo:2938; depth 50.72 2024-11-06T04:55:04Z- handleDVL() - DVL msg epochMillisec: 1730868904.35 2024-11-06T04:55:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:55:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:55:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:55:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:55:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:55:04Z- handleDvl() - RDI_Pathfinder msg: 1730868904.35 sec; seqNo:1066; #beams:0 2024-11-06T04:55:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:55:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.03 , -0.02 2024-11-06T04:55:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868904.36 epoch sec; seqNo:2938; 4061730.94 north; 594921.97 east; 36.70 lat; -121.94 lon; 50.72 depth; utmZone: 10 2024-11-06T04:55:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868903.54 2024-11-06T04:55:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:04Z- waiting for 1730868907.54, time is 1730868904.36 2024-11-06T04:55:04Z- AHRS_M2 msg: 1730868904.76 epoch sec; seqNo:2758 2024-11-06T04:55:04Z- AHRS_M2 msg: 0.38 phi; -0.12 theta; 2.62 psi 2024-11-06T04:55:04Z- handleDepth()- Depth_Keller msg: 1730868904.76 epoch sec; seqNo:2939; depth 50.75 2024-11-06T04:55:04Z- handleDVL() - DVL msg epochMillisec: 1730868904.78 2024-11-06T04:55:04Z- handleDvl() - RDI_Pathfinder msg: 1730868904.78 sec; seqNo:1067; #beams:4 2024-11-06T04:55:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.79 , 37.82 , 44.72 , 49.47 2024-11-06T04:55:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.77 , -0.00 , -0.04 2024-11-06T04:55:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868904.80 epoch sec; seqNo:2939; 4061730.94 north; 594921.97 east; 36.70 lat; -121.94 lon; 50.75 depth; utmZone: 10 2024-11-06T04:55:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868903.54 2024-11-06T04:55:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:04Z- waiting for 1730868907.54, time is 1730868904.80 2024-11-06T04:55:05Z- AHRS_M2 msg: 1730868905.14 epoch sec; seqNo:2759 2024-11-06T04:55:05Z- AHRS_M2 msg: 0.38 phi; -0.11 theta; 2.66 psi 2024-11-06T04:55:05Z- handleDepth()- Depth_Keller msg: 1730868905.15 epoch sec; seqNo:2940; depth 50.80 2024-11-06T04:55:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868905.16 epoch sec; seqNo:2940; 4061730.56 north; 594921.98 east; 36.70 lat; -121.94 lon; 50.80 depth; utmZone: 10 2024-11-06T04:55:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868903.54 2024-11-06T04:55:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:05Z- waiting for 1730868907.54, time is 1730868905.16 2024-11-06T04:55:05Z- AHRS_M2 msg: 1730868905.54 epoch sec; seqNo:2760 2024-11-06T04:55:05Z- AHRS_M2 msg: 0.38 phi; -0.11 theta; 2.70 psi 2024-11-06T04:55:05Z- handleDepth()- Depth_Keller msg: 1730868905.55 epoch sec; seqNo:2941; depth 50.82 2024-11-06T04:55:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868905.56 epoch sec; seqNo:2941; 4061730.18 north; 594921.98 east; 36.70 lat; -121.94 lon; 50.82 depth; utmZone: 10 2024-11-06T04:55:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868903.54 2024-11-06T04:55:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:05Z- waiting for 1730868907.54, time is 1730868905.56 2024-11-06T04:55:05Z- AHRS_M2 msg: 1730868905.95 epoch sec; seqNo:2761 2024-11-06T04:55:05Z- AHRS_M2 msg: 0.38 phi; -0.10 theta; 2.73 psi 2024-11-06T04:55:05Z- handleDepth()- Depth_Keller msg: 1730868905.96 epoch sec; seqNo:2942; depth 50.87 2024-11-06T04:55:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868905.97 epoch sec; seqNo:2942; 4061729.80 north; 594921.98 east; 36.70 lat; -121.94 lon; 50.87 depth; utmZone: 10 2024-11-06T04:55:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868903.54 2024-11-06T04:55:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:06Z- waiting for 1730868907.54, time is 1730868905.97 2024-11-06T04:55:06Z- AHRS_M2 msg: 1730868906.35 epoch sec; seqNo:2762 2024-11-06T04:55:06Z- AHRS_M2 msg: 0.39 phi; -0.10 theta; 2.77 psi 2024-11-06T04:55:06Z- handleDepth()- Depth_Keller msg: 1730868906.36 epoch sec; seqNo:2943; depth 50.89 2024-11-06T04:55:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868906.37 epoch sec; seqNo:2943; 4061729.80 north; 594921.98 east; 36.70 lat; -121.94 lon; 50.89 depth; utmZone: 10 2024-11-06T04:55:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868903.54 2024-11-06T04:55:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:06Z- waiting for 1730868907.54, time is 1730868906.37 2024-11-06T04:55:06Z- AHRS_M2 msg: 1730868906.76 epoch sec; seqNo:2763 2024-11-06T04:55:06Z- AHRS_M2 msg: 0.39 phi; -0.09 theta; 2.81 psi 2024-11-06T04:55:06Z- handleDepth()- Depth_Keller msg: 1730868906.77 epoch sec; seqNo:2944; depth 50.94 2024-11-06T04:55:06Z- handleDVL() - DVL msg epochMillisec: 1730868906.80 2024-11-06T04:55:06Z- handleDvl() - RDI_Pathfinder msg: 1730868906.80 sec; seqNo:1068; #beams:4 2024-11-06T04:55:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.50 , 37.77 , 45.50 , 47.92 2024-11-06T04:55:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.03 , -0.03 2024-11-06T04:55:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868906.81 epoch sec; seqNo:2944; 4061729.42 north; 594921.99 east; 36.70 lat; -121.94 lon; 50.94 depth; utmZone: 10 2024-11-06T04:55:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868903.54 2024-11-06T04:55:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:06Z- waiting for 1730868907.54, time is 1730868906.81 2024-11-06T04:55:07Z- handleDepth()- Depth_Keller msg: 1730868907.21 epoch sec; seqNo:2945; depth 50.97 2024-11-06T04:55:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868907.22 epoch sec; seqNo:2945; 4061729.04 north; 594921.99 east; 36.70 lat; -121.94 lon; 50.97 depth; utmZone: 10 2024-11-06T04:55:07Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:55:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868903.54 2024-11-06T04:55:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:07Z- waiting for 1730868907.54, time is 1730868907.22 2024-11-06T04:55:07Z- handleDepth()- Depth_Keller msg: 1730868907.58 epoch sec; seqNo:2946; depth 51.00 2024-11-06T04:55:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868907.59 epoch sec; seqNo:2946; 4061728.66 north; 594922.00 east; 36.70 lat; -121.94 lon; 51.00 depth; utmZone: 10 2024-11-06T04:55:07Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:55:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868903.54 2024-11-06T04:55:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:07Z- waiting for 1730868907.54, time is 1730868907.59 2024-11-06T04:55:07Z- LcmTrn::updateTrn() >>>> pose time 1730868907.59, meas time 1730868906.80 <<<< 2024-11-06T04:55:07Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868907.59 2024-11-06T04:55:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868906.80 sec, ping # 0; waiting for INS data more recent than 1730868903.54... 2024-11-06T04:55:07Z- TerrainNav::motionUpdate() - currEstimate = (4061729, 594922.00, 51.00) 2024-11-06T04:55:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:55:07Z- TerrainNav:: SubcloudNIS is 0.57, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:55:07Z- TNavPF::Projecting Measurements 2024-11-06T04:55:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:55:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:55:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:55:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:55:07Z- TNavFilter::4 good beams. 2024-11-06T04:55:07Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:55:07Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:55:07Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:55:07Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:55:07Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:55:07Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:55:07Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:55:07Z- TerrainMapOctree: (vehN, vehE ) = (4061729.26, 594921.75). 2024-11-06T04:55:07Z- TerrainMapOctree: Min Distance = 173.00. 2024-11-06T04:55:07Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:55:07Z- Forcing Subcloud Comparison 2024-11-06T04:55:07Z- Weighting particles with subcloud comparison 2024-11-06T04:55:07Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:07Z- meanExpectedMeasDiff: 1.363621 delta_rms_squared: 2.571477 alpha: 0.992282 2024-11-06T04:55:07Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:07Z- meanExpectedMeasDiff: 0.255610 delta_rms_squared: 0.961726 alpha: 0.979628 2024-11-06T04:55:07Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:07Z- meanExpectedMeasDiff: 1.338209 delta_rms_squared: 1.373114 alpha: 0.985644 2024-11-06T04:55:07Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:07Z- meanExpectedMeasDiff: -1.642868 delta_rms_squared: 1.687780 alpha: 0.988289 2024-11-06T04:55:07Z- Subcloud NIS: 0.542287 2024-11-06T04:55:07Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:55:07Z- TNavPF:: sumWeights = 0.405600 2024-11-06T04:55:07Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:55:07Z- TNavPF::First Term of Map Covariance Matrix is 0.70 2024-11-06T04:55:07Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T04:55:07Z- TerrainNav::Resampling particle filter... 2024-11-06T04:55:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868906.80 sec. 2024-11-06T04:55:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:55:07Z- variances([0], [2], and psi_berg) = 0.554944 0.200777 0.000000 2024-11-06T04:55:07Z- publishEstimatesDecoder() - at 1730868907.59 sec 2024-11-06T04:55:07Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:55:07Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061673.73 594926.37 58.97 2024-11-06T04:55:07Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061672.16 594926.58 59.65 lat/lon: 36.70 -121.94 2024-11-06T04:55:07Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.55 0.20 0.70 0.00 2024-11-06T04:55:08Z- AHRS_M2 msg: 1730868908.00 epoch sec; seqNo:2764 2024-11-06T04:55:08Z- AHRS_M2 msg: 0.39 phi; -0.07 theta; 2.96 psi 2024-11-06T04:55:08Z- handleDepth()- Depth_Keller msg: 1730868908.01 epoch sec; seqNo:2947; depth 51.04 2024-11-06T04:55:08Z- handleDVL() - DVL msg epochMillisec: 1730868908.01 2024-11-06T04:55:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:55:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:55:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:55:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:55:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:55:08Z- handleDvl() - RDI_Pathfinder msg: 1730868908.01 sec; seqNo:1069; #beams:0 2024-11-06T04:55:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:55:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.75 , 0.03 , -0.03 2024-11-06T04:55:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868908.03 epoch sec; seqNo:2947; 4061728.29 north; 594922.00 east; 36.70 lat; -121.94 lon; 51.04 depth; utmZone: 10 2024-11-06T04:55:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868907.59 2024-11-06T04:55:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:08Z- waiting for 1730868911.59, time is 1730868908.03 2024-11-06T04:55:08Z- AHRS_M2 msg: 1730868908.37 epoch sec; seqNo:2765 2024-11-06T04:55:08Z- AHRS_M2 msg: 0.38 phi; -0.07 theta; 3.00 psi 2024-11-06T04:55:08Z- handleDepth()- Depth_Keller msg: 1730868908.38 epoch sec; seqNo:2948; depth 51.06 2024-11-06T04:55:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868908.39 epoch sec; seqNo:2948; 4061728.29 north; 594922.00 east; 36.70 lat; -121.94 lon; 51.06 depth; utmZone: 10 2024-11-06T04:55:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868907.59 2024-11-06T04:55:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:08Z- waiting for 1730868911.59, time is 1730868908.39 2024-11-06T04:55:08Z- AHRS_M2 msg: 1730868908.78 epoch sec; seqNo:2766 2024-11-06T04:55:08Z- AHRS_M2 msg: 0.38 phi; -0.06 theta; 3.04 psi 2024-11-06T04:55:08Z- handleDepth()- Depth_Keller msg: 1730868908.79 epoch sec; seqNo:2949; depth 51.09 2024-11-06T04:55:08Z- handleDVL() - DVL msg epochMillisec: 1730868908.81 2024-11-06T04:55:08Z- handleDvl() - RDI_Pathfinder msg: 1730868908.81 sec; seqNo:1070; #beams:4 2024-11-06T04:55:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.50 , 37.54 , 46.16 , 47.92 2024-11-06T04:55:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , 0.06 , 0.01 2024-11-06T04:55:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868908.84 epoch sec; seqNo:2949; 4061727.91 north; 594922.00 east; 36.70 lat; -121.94 lon; 51.09 depth; utmZone: 10 2024-11-06T04:55:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868907.59 2024-11-06T04:55:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:08Z- waiting for 1730868911.59, time is 1730868908.84 2024-11-06T04:55:09Z- AHRS_M2 msg: 1730868909.18 epoch sec; seqNo:2767 2024-11-06T04:55:09Z- AHRS_M2 msg: 0.39 phi; -0.06 theta; 3.07 psi 2024-11-06T04:55:09Z- handleDepth()- Depth_Keller msg: 1730868909.18 epoch sec; seqNo:2950; depth 51.12 2024-11-06T04:55:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868909.20 epoch sec; seqNo:2950; 4061727.53 north; 594922.01 east; 36.70 lat; -121.94 lon; 51.12 depth; utmZone: 10 2024-11-06T04:55:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868907.59 2024-11-06T04:55:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:09Z- waiting for 1730868911.59, time is 1730868909.20 2024-11-06T04:55:09Z- AHRS_M2 msg: 1730868909.58 epoch sec; seqNo:2768 2024-11-06T04:55:09Z- AHRS_M2 msg: 0.38 phi; -0.06 theta; 3.11 psi 2024-11-06T04:55:09Z- handleDepth()- Depth_Keller msg: 1730868909.59 epoch sec; seqNo:2951; depth 51.15 2024-11-06T04:55:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868909.61 epoch sec; seqNo:2951; 4061727.15 north; 594922.01 east; 36.70 lat; -121.94 lon; 51.15 depth; utmZone: 10 2024-11-06T04:55:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868907.59 2024-11-06T04:55:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:09Z- waiting for 1730868911.59, time is 1730868909.61 2024-11-06T04:55:09Z- AHRS_M2 msg: 1730868909.99 epoch sec; seqNo:2769 2024-11-06T04:55:09Z- AHRS_M2 msg: 0.38 phi; -0.05 theta; 3.15 psi 2024-11-06T04:55:09Z- handleDepth()- Depth_Keller msg: 1730868909.99 epoch sec; seqNo:2952; depth 51.18 2024-11-06T04:55:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868910.00 epoch sec; seqNo:2952; 4061727.15 north; 594922.01 east; 36.70 lat; -121.94 lon; 51.18 depth; utmZone: 10 2024-11-06T04:55:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868907.59 2024-11-06T04:55:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:10Z- waiting for 1730868911.59, time is 1730868910.00 2024-11-06T04:55:10Z- AHRS_M2 msg: 1730868910.39 epoch sec; seqNo:2770 2024-11-06T04:55:10Z- AHRS_M2 msg: 0.38 phi; -0.05 theta; 3.18 psi 2024-11-06T04:55:10Z- handleDepth()- Depth_Keller msg: 1730868910.40 epoch sec; seqNo:2953; depth 51.21 2024-11-06T04:55:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868910.41 epoch sec; seqNo:2953; 4061726.77 north; 594922.02 east; 36.70 lat; -121.94 lon; 51.21 depth; utmZone: 10 2024-11-06T04:55:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868907.59 2024-11-06T04:55:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:10Z- waiting for 1730868911.59, time is 1730868910.41 2024-11-06T04:55:10Z- AHRS_M2 msg: 1730868910.80 epoch sec; seqNo:2771 2024-11-06T04:55:10Z- AHRS_M2 msg: 0.38 phi; -0.05 theta; 3.22 psi 2024-11-06T04:55:10Z- handleDepth()- Depth_Keller msg: 1730868910.80 epoch sec; seqNo:2954; depth 51.23 2024-11-06T04:55:10Z- handleDVL() - DVL msg epochMillisec: 1730868910.84 2024-11-06T04:55:10Z- handleDvl() - RDI_Pathfinder msg: 1730868910.84 sec; seqNo:1071; #beams:4 2024-11-06T04:55:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.28 , 37.32 , 46.82 , 47.71 2024-11-06T04:55:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.03 , 0.04 2024-11-06T04:55:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868910.85 epoch sec; seqNo:2954; 4061726.39 north; 594922.02 east; 36.70 lat; -121.94 lon; 51.23 depth; utmZone: 10 2024-11-06T04:55:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868907.59 2024-11-06T04:55:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:10Z- waiting for 1730868911.59, time is 1730868910.85 2024-11-06T04:55:11Z- AHRS_M2 msg: 1730868911.20 epoch sec; seqNo:2772 2024-11-06T04:55:11Z- AHRS_M2 msg: 0.37 phi; -0.04 theta; 3.26 psi 2024-11-06T04:55:11Z- handleDepth()- Depth_Keller msg: 1730868911.21 epoch sec; seqNo:2955; depth 51.25 2024-11-06T04:55:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868911.23 epoch sec; seqNo:2955; 4061726.01 north; 594922.03 east; 36.70 lat; -121.94 lon; 51.25 depth; utmZone: 10 2024-11-06T04:55:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868907.59 2024-11-06T04:55:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:11Z- waiting for 1730868911.59, time is 1730868911.23 2024-11-06T04:55:11Z- AHRS_M2 msg: 1730868911.61 epoch sec; seqNo:2773 2024-11-06T04:55:11Z- AHRS_M2 msg: 0.38 phi; -0.04 theta; 3.29 psi 2024-11-06T04:55:11Z- handleDepth()- Depth_Keller msg: 1730868911.61 epoch sec; seqNo:2956; depth 51.29 2024-11-06T04:55:11Z- handleDVL() - DVL msg epochMillisec: 1730868911.61 2024-11-06T04:55:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:55:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:55:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:55:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:55:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:55:11Z- handleDvl() - RDI_Pathfinder msg: 1730868911.61 sec; seqNo:1072; #beams:0 2024-11-06T04:55:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:55:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.03 , 0.04 2024-11-06T04:55:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868911.62 epoch sec; seqNo:2956; 4061725.63 north; 594922.03 east; 36.70 lat; -121.94 lon; 51.29 depth; utmZone: 10 2024-11-06T04:55:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868907.59 2024-11-06T04:55:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:11Z- waiting for 1730868911.59, time is 1730868911.62 2024-11-06T04:55:11Z- LcmTrn::updateTrn() >>>> pose time 1730868911.62, meas time 1730868911.61 <<<< 2024-11-06T04:55:11Z- LcmTrn::updateTrn() equating pose 1730868911.61 and meas times 1730868911.61 2024-11-06T04:55:11Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868911.61 2024-11-06T04:55:11Z- TerrainNav::motionUpdate() - currEstimate = (4061726, 594922.03, 51.29) 2024-11-06T04:55:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:55:11Z- TerrainNav:: SubcloudNIS is 0.54, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:55:11Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:55:11Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:55:11Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:55:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:55:11Z- variances([0], [2], and psi_berg) = 0.555917 0.201733 0.000000 2024-11-06T04:55:11Z- publishEstimatesDecoder() - at 1730868911.61 sec 2024-11-06T04:55:11Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:55:11Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061670.72 594926.33 59.47 2024-11-06T04:55:11Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061669.13 594926.61 59.93 lat/lon: 36.70 -121.94 2024-11-06T04:55:11Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.56 0.20 0.96 0.00 2024-11-06T04:55:12Z- AHRS_M2 msg: 1730868912.01 epoch sec; seqNo:2774 2024-11-06T04:55:12Z- AHRS_M2 msg: 0.38 phi; -0.04 theta; 3.33 psi 2024-11-06T04:55:12Z- handleDepth()- Depth_Keller msg: 1730868912.02 epoch sec; seqNo:2957; depth 51.30 2024-11-06T04:55:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868912.03 epoch sec; seqNo:2957; 4061725.25 north; 594922.03 east; 36.70 lat; -121.94 lon; 51.30 depth; utmZone: 10 2024-11-06T04:55:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868911.61 2024-11-06T04:55:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:12Z- waiting for 1730868915.61, time is 1730868912.03 2024-11-06T04:55:12Z- AHRS_M2 msg: 1730868912.42 epoch sec; seqNo:2775 2024-11-06T04:55:12Z- AHRS_M2 msg: 0.39 phi; -0.03 theta; 3.37 psi 2024-11-06T04:55:12Z- handleDepth()- Depth_Keller msg: 1730868912.42 epoch sec; seqNo:2958; depth 51.33 2024-11-06T04:55:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868912.43 epoch sec; seqNo:2958; 4061725.25 north; 594922.03 east; 36.70 lat; -121.94 lon; 51.33 depth; utmZone: 10 2024-11-06T04:55:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868911.61 2024-11-06T04:55:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:12Z- waiting for 1730868915.61, time is 1730868912.43 2024-11-06T04:55:12Z- handleDepth()- Depth_Keller msg: 1730868912.84 epoch sec; seqNo:2959; depth 51.36 2024-11-06T04:55:12Z- handleDVL() - DVL msg epochMillisec: 1730868912.86 2024-11-06T04:55:12Z- handleDvl() - RDI_Pathfinder msg: 1730868912.86 sec; seqNo:1073; #beams:4 2024-11-06T04:55:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 61.62 , 37.99 , 47.04 , 47.04 2024-11-06T04:55:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.01 , 0.04 2024-11-06T04:55:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868912.87 epoch sec; seqNo:2959; 4061724.88 north; 594922.04 east; 36.70 lat; -121.94 lon; 51.36 depth; utmZone: 10 2024-11-06T04:55:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868911.61 2024-11-06T04:55:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:12Z- waiting for 1730868915.61, time is 1730868912.87 2024-11-06T04:55:13Z- AHRS_M2 msg: 1730868913.22 epoch sec; seqNo:2776 2024-11-06T04:55:13Z- AHRS_M2 msg: 0.39 phi; -0.03 theta; 3.44 psi 2024-11-06T04:55:13Z- handleDepth()- Depth_Keller msg: 1730868913.23 epoch sec; seqNo:2960; depth 51.38 2024-11-06T04:55:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868913.24 epoch sec; seqNo:2960; 4061724.50 north; 594922.04 east; 36.70 lat; -121.94 lon; 51.38 depth; utmZone: 10 2024-11-06T04:55:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868911.61 2024-11-06T04:55:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:13Z- waiting for 1730868915.61, time is 1730868913.24 2024-11-06T04:55:13Z- AHRS_M2 msg: 1730868913.63 epoch sec; seqNo:2777 2024-11-06T04:55:13Z- AHRS_M2 msg: 0.39 phi; -0.03 theta; 3.48 psi 2024-11-06T04:55:13Z- handleDepth()- Depth_Keller msg: 1730868913.64 epoch sec; seqNo:2961; depth 51.39 2024-11-06T04:55:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868913.65 epoch sec; seqNo:2961; 4061724.12 north; 594922.05 east; 36.70 lat; -121.94 lon; 51.39 depth; utmZone: 10 2024-11-06T04:55:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868911.61 2024-11-06T04:55:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:13Z- waiting for 1730868915.61, time is 1730868913.65 2024-11-06T04:55:14Z- AHRS_M2 msg: 1730868914.03 epoch sec; seqNo:2778 2024-11-06T04:55:14Z- AHRS_M2 msg: 0.39 phi; -0.03 theta; 3.51 psi 2024-11-06T04:55:14Z- handleDepth()- Depth_Keller msg: 1730868914.03 epoch sec; seqNo:2962; depth 51.39 2024-11-06T04:55:14Z- handleDVL() - DVL msg epochMillisec: 1730868914.04 2024-11-06T04:55:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:55:14Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:55:14Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:55:14Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:55:14Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:55:14Z- handleDvl() - RDI_Pathfinder msg: 1730868914.04 sec; seqNo:1074; #beams:0 2024-11-06T04:55:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:55:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.01 , 0.04 2024-11-06T04:55:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868914.04 epoch sec; seqNo:2962; 4061723.74 north; 594922.05 east; 36.70 lat; -121.94 lon; 51.39 depth; utmZone: 10 2024-11-06T04:55:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868911.61 2024-11-06T04:55:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:14Z- waiting for 1730868915.61, time is 1730868914.04 2024-11-06T04:55:14Z- AHRS_M2 msg: 1730868914.43 epoch sec; seqNo:2779 2024-11-06T04:55:14Z- AHRS_M2 msg: 0.38 phi; -0.02 theta; 3.55 psi 2024-11-06T04:55:14Z- handleDepth()- Depth_Keller msg: 1730868914.44 epoch sec; seqNo:2963; depth 51.41 2024-11-06T04:55:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868914.45 epoch sec; seqNo:2963; 4061723.74 north; 594922.05 east; 36.70 lat; -121.94 lon; 51.41 depth; utmZone: 10 2024-11-06T04:55:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868911.61 2024-11-06T04:55:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:14Z- waiting for 1730868915.61, time is 1730868914.45 2024-11-06T04:55:14Z- AHRS_M2 msg: 1730868914.84 epoch sec; seqNo:2780 2024-11-06T04:55:14Z- AHRS_M2 msg: 0.38 phi; -0.02 theta; 3.59 psi 2024-11-06T04:55:14Z- handleDepth()- Depth_Keller msg: 1730868914.85 epoch sec; seqNo:2964; depth 51.42 2024-11-06T04:55:14Z- handleDVL() - DVL msg epochMillisec: 1730868914.87 2024-11-06T04:55:14Z- handleDvl() - RDI_Pathfinder msg: 1730868914.87 sec; seqNo:1075; #beams:4 2024-11-06T04:55:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.28 , 37.99 , 47.71 , 46.82 2024-11-06T04:55:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.06 , -0.01 2024-11-06T04:55:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868914.88 epoch sec; seqNo:2964; 4061723.36 north; 594922.06 east; 36.70 lat; -121.94 lon; 51.42 depth; utmZone: 10 2024-11-06T04:55:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868911.61 2024-11-06T04:55:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:15Z- waiting for 1730868915.61, time is 1730868914.88 2024-11-06T04:55:15Z- AHRS_M2 msg: 1730868915.24 epoch sec; seqNo:2781 2024-11-06T04:55:15Z- AHRS_M2 msg: 0.38 phi; -0.02 theta; 3.63 psi 2024-11-06T04:55:15Z- handleDepth()- Depth_Keller msg: 1730868915.24 epoch sec; seqNo:2965; depth 51.42 2024-11-06T04:55:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868915.27 epoch sec; seqNo:2965; 4061722.98 north; 594922.06 east; 36.70 lat; -121.94 lon; 51.42 depth; utmZone: 10 2024-11-06T04:55:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868911.61 2024-11-06T04:55:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:15Z- waiting for 1730868915.61, time is 1730868915.27 2024-11-06T04:55:15Z- handleDepth()- Depth_Keller msg: 1730868915.66 epoch sec; seqNo:2966; depth 51.44 2024-11-06T04:55:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868915.67 epoch sec; seqNo:2966; 4061722.59 north; 594920.84 east; 36.70 lat; -121.94 lon; 51.44 depth; utmZone: 10 2024-11-06T04:55:15Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:55:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868911.61 2024-11-06T04:55:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:15Z- waiting for 1730868915.61, time is 1730868915.67 2024-11-06T04:55:15Z- LcmTrn::updateTrn() >>>> pose time 1730868915.67, meas time 1730868914.87 <<<< 2024-11-06T04:55:15Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868915.67 2024-11-06T04:55:15Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868914.87 sec, ping # 0; waiting for INS data more recent than 1730868911.61... 2024-11-06T04:55:15Z- TerrainNav::motionUpdate() - currEstimate = (4061723, 594920.84, 51.44) 2024-11-06T04:55:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:55:15Z- TerrainNav:: SubcloudNIS is 0.54, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:55:15Z- TNavPF::Projecting Measurements 2024-11-06T04:55:15Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:55:15Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:55:15Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:55:15Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:55:15Z- TNavFilter::4 good beams. 2024-11-06T04:55:15Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:55:15Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:55:15Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:55:15Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:55:15Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:55:15Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:55:15Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:55:15Z- TerrainMapOctree: (vehN, vehE ) = (4061723.20, 594921.08). 2024-11-06T04:55:15Z- TerrainMapOctree: Min Distance = 173.17. 2024-11-06T04:55:15Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:55:15Z- Forcing Subcloud Comparison 2024-11-06T04:55:15Z- Weighting particles with subcloud comparison 2024-11-06T04:55:15Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:15Z- meanExpectedMeasDiff: 2.962705 delta_rms_squared: 3.183626 alpha: 0.993757 2024-11-06T04:55:15Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:15Z- meanExpectedMeasDiff: 0.977395 delta_rms_squared: 1.234811 alpha: 0.984061 2024-11-06T04:55:15Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:15Z- meanExpectedMeasDiff: 2.812774 delta_rms_squared: 1.821541 alpha: 0.989140 2024-11-06T04:55:15Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:15Z- meanExpectedMeasDiff: 0.531066 delta_rms_squared: 1.937365 alpha: 0.989782 2024-11-06T04:55:15Z- Subcloud NIS: 0.540981 2024-11-06T04:55:15Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:55:15Z- TNavPF:: sumWeights = 0.297121 2024-11-06T04:55:15Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:55:15Z- TNavPF::First Term of Map Covariance Matrix is 0.79 2024-11-06T04:55:15Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.02 2024-11-06T04:55:15Z- TerrainNav::Resampling particle filter... 2024-11-06T04:55:15Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868914.87 sec. 2024-11-06T04:55:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:55:15Z- variances([0], [2], and psi_berg) = 0.569309 0.207581 0.000000 2024-11-06T04:55:15Z- publishEstimatesDecoder() - at 1730868915.67 sec 2024-11-06T04:55:15Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:55:15Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061667.70 594925.16 58.76 2024-11-06T04:55:15Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061666.06 594925.42 59.27 lat/lon: 36.70 -121.94 2024-11-06T04:55:15Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.21 0.76 0.00 2024-11-06T04:55:16Z- AHRS_M2 msg: 1730868916.09 epoch sec; seqNo:2782 2024-11-06T04:55:16Z- AHRS_M2 msg: 0.38 phi; -0.02 theta; 3.70 psi 2024-11-06T04:55:16Z- handleDepth()- Depth_Keller msg: 1730868916.12 epoch sec; seqNo:2967; depth 51.44 2024-11-06T04:55:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868916.14 epoch sec; seqNo:2967; 4061722.21 north; 594920.85 east; 36.70 lat; -121.94 lon; 51.44 depth; utmZone: 10 2024-11-06T04:55:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868915.67 2024-11-06T04:55:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:16Z- waiting for 1730868919.67, time is 1730868916.14 2024-11-06T04:55:16Z- AHRS_M2 msg: 1730868916.45 epoch sec; seqNo:2783 2024-11-06T04:55:16Z- AHRS_M2 msg: 0.39 phi; -0.02 theta; 3.74 psi 2024-11-06T04:55:16Z- handleDepth()- Depth_Keller msg: 1730868916.46 epoch sec; seqNo:2968; depth 51.44 2024-11-06T04:55:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868916.47 epoch sec; seqNo:2968; 4061722.21 north; 594920.85 east; 36.70 lat; -121.94 lon; 51.44 depth; utmZone: 10 2024-11-06T04:55:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868915.67 2024-11-06T04:55:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:16Z- waiting for 1730868919.67, time is 1730868916.47 2024-11-06T04:55:16Z- AHRS_M2 msg: 1730868916.86 epoch sec; seqNo:2784 2024-11-06T04:55:16Z- AHRS_M2 msg: 0.39 phi; -0.02 theta; 3.78 psi 2024-11-06T04:55:16Z- handleDepth()- Depth_Keller msg: 1730868916.87 epoch sec; seqNo:2969; depth 51.44 2024-11-06T04:55:16Z- handleDVL() - DVL msg epochMillisec: 1730868916.88 2024-11-06T04:55:16Z- handleDvl() - RDI_Pathfinder msg: 1730868916.88 sec; seqNo:1076; #beams:4 2024-11-06T04:55:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.72 , 38.21 , 47.71 , 47.04 2024-11-06T04:55:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.05 , -0.05 2024-11-06T04:55:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868916.90 epoch sec; seqNo:2969; 4061721.83 north; 594920.85 east; 36.70 lat; -121.94 lon; 51.44 depth; utmZone: 10 2024-11-06T04:55:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868915.67 2024-11-06T04:55:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:17Z- waiting for 1730868919.67, time is 1730868916.90 2024-11-06T04:55:17Z- AHRS_M2 msg: 1730868917.26 epoch sec; seqNo:2785 2024-11-06T04:55:17Z- AHRS_M2 msg: 0.40 phi; -0.02 theta; 3.81 psi 2024-11-06T04:55:17Z- handleDepth()- Depth_Keller msg: 1730868917.27 epoch sec; seqNo:2970; depth 51.45 2024-11-06T04:55:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868917.30 epoch sec; seqNo:2970; 4061721.45 north; 594920.86 east; 36.70 lat; -121.94 lon; 51.45 depth; utmZone: 10 2024-11-06T04:55:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868915.67 2024-11-06T04:55:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:17Z- waiting for 1730868919.67, time is 1730868917.30 2024-11-06T04:55:17Z- AHRS_M2 msg: 1730868917.67 epoch sec; seqNo:2786 2024-11-06T04:55:17Z- AHRS_M2 msg: 0.40 phi; -0.02 theta; 3.85 psi 2024-11-06T04:55:17Z- handleDepth()- Depth_Keller msg: 1730868917.67 epoch sec; seqNo:2971; depth 51.47 2024-11-06T04:55:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868917.68 epoch sec; seqNo:2971; 4061721.45 north; 594920.86 east; 36.70 lat; -121.94 lon; 51.47 depth; utmZone: 10 2024-11-06T04:55:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868915.67 2024-11-06T04:55:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:17Z- waiting for 1730868919.67, time is 1730868917.68 2024-11-06T04:55:18Z- AHRS_M2 msg: 1730868918.07 epoch sec; seqNo:2787 2024-11-06T04:55:18Z- AHRS_M2 msg: 0.39 phi; -0.02 theta; 3.89 psi 2024-11-06T04:55:18Z- handleDepth()- Depth_Keller msg: 1730868918.08 epoch sec; seqNo:2972; depth 51.46 2024-11-06T04:55:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868918.09 epoch sec; seqNo:2972; 4061721.07 north; 594920.86 east; 36.70 lat; -121.94 lon; 51.46 depth; utmZone: 10 2024-11-06T04:55:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868915.67 2024-11-06T04:55:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:18Z- waiting for 1730868919.67, time is 1730868918.09 2024-11-06T04:55:18Z- AHRS_M2 msg: 1730868918.47 epoch sec; seqNo:2788 2024-11-06T04:55:18Z- AHRS_M2 msg: 0.38 phi; -0.01 theta; 3.92 psi 2024-11-06T04:55:18Z- handleDepth()- Depth_Keller msg: 1730868918.49 epoch sec; seqNo:2973; depth 51.46 2024-11-06T04:55:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868918.51 epoch sec; seqNo:2973; 4061721.06 north; 594919.64 east; 36.70 lat; -121.94 lon; 51.46 depth; utmZone: 10 2024-11-06T04:55:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868915.67 2024-11-06T04:55:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:18Z- waiting for 1730868919.67, time is 1730868918.51 2024-11-06T04:55:18Z- AHRS_M2 msg: 1730868918.88 epoch sec; seqNo:2789 2024-11-06T04:55:18Z- AHRS_M2 msg: 0.37 phi; -0.01 theta; 3.96 psi 2024-11-06T04:55:18Z- handleDepth()- Depth_Keller msg: 1730868918.88 epoch sec; seqNo:2974; depth 51.47 2024-11-06T04:55:18Z- handleDVL() - DVL msg epochMillisec: 1730868918.91 2024-11-06T04:55:18Z- handleDvl() - RDI_Pathfinder msg: 1730868918.91 sec; seqNo:1077; #beams:4 2024-11-06T04:55:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.72 , 37.99 , 47.71 , 47.48 2024-11-06T04:55:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , 0.03 , -0.05 2024-11-06T04:55:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868918.92 epoch sec; seqNo:2974; 4061720.68 north; 594919.64 east; 36.70 lat; -121.94 lon; 51.47 depth; utmZone: 10 2024-11-06T04:55:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868915.67 2024-11-06T04:55:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:19Z- waiting for 1730868919.67, time is 1730868918.92 2024-11-06T04:55:19Z- AHRS_M2 msg: 1730868919.28 epoch sec; seqNo:2790 2024-11-06T04:55:19Z- AHRS_M2 msg: 0.36 phi; -0.01 theta; 3.99 psi 2024-11-06T04:55:19Z- handleDepth()- Depth_Keller msg: 1730868919.29 epoch sec; seqNo:2975; depth 51.47 2024-11-06T04:55:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868919.31 epoch sec; seqNo:2975; 4061720.68 north; 594919.64 east; 36.70 lat; -121.94 lon; 51.47 depth; utmZone: 10 2024-11-06T04:55:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868915.67 2024-11-06T04:55:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:19Z- waiting for 1730868919.67, time is 1730868919.31 2024-11-06T04:55:19Z- AHRS_M2 msg: 1730868919.69 epoch sec; seqNo:2791 2024-11-06T04:55:19Z- AHRS_M2 msg: 0.36 phi; -0.00 theta; 4.02 psi 2024-11-06T04:55:19Z- handleDepth()- Depth_Keller msg: 1730868919.69 epoch sec; seqNo:2976; depth 51.47 2024-11-06T04:55:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868919.70 epoch sec; seqNo:2976; 4061720.30 north; 594919.65 east; 36.70 lat; -121.94 lon; 51.47 depth; utmZone: 10 2024-11-06T04:55:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868915.67 2024-11-06T04:55:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:19Z- waiting for 1730868919.67, time is 1730868919.70 2024-11-06T04:55:19Z- LcmTrn::updateTrn() >>>> pose time 1730868919.70, meas time 1730868918.91 <<<< 2024-11-06T04:55:19Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868919.70 2024-11-06T04:55:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868918.91 sec, ping # 0; waiting for INS data more recent than 1730868915.67... 2024-11-06T04:55:19Z- TerrainNav::motionUpdate() - currEstimate = (4061720, 594919.65, 51.47) 2024-11-06T04:55:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:55:19Z- TerrainNav:: SubcloudNIS is 0.54, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:55:19Z- TNavPF::Projecting Measurements 2024-11-06T04:55:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:55:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:55:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:55:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:55:19Z- TNavFilter::4 good beams. 2024-11-06T04:55:19Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:55:19Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:55:19Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:55:19Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:55:19Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:55:19Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:55:19Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:55:19Z- TerrainMapOctree: (vehN, vehE ) = (4061720.75, 594919.88). 2024-11-06T04:55:19Z- TerrainMapOctree: Min Distance = 172.38. 2024-11-06T04:55:19Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:55:19Z- Forcing Subcloud Comparison 2024-11-06T04:55:19Z- Weighting particles with subcloud comparison 2024-11-06T04:55:19Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:19Z- meanExpectedMeasDiff: 3.635481 delta_rms_squared: 2.512320 alpha: 0.992102 2024-11-06T04:55:19Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:19Z- meanExpectedMeasDiff: 0.108134 delta_rms_squared: 1.026580 alpha: 0.980890 2024-11-06T04:55:19Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:19Z- meanExpectedMeasDiff: 1.720394 delta_rms_squared: 1.517685 alpha: 0.986993 2024-11-06T04:55:19Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:19Z- meanExpectedMeasDiff: 1.107256 delta_rms_squared: 1.600927 alpha: 0.987661 2024-11-06T04:55:19Z- Subcloud NIS: 0.506835 2024-11-06T04:55:19Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:55:19Z- TNavPF:: sumWeights = 0.321703 2024-11-06T04:55:19Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:55:20Z- TNavPF::First Term of Map Covariance Matrix is 1.39 2024-11-06T04:55:20Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T04:55:20Z- TerrainNav::Resampling particle filter... 2024-11-06T04:55:20Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868918.91 sec. 2024-11-06T04:55:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:55:20Z- variances([0], [2], and psi_berg) = 0.578832 0.206496 0.000000 2024-11-06T04:55:20Z- publishEstimatesDecoder() - at 1730868919.70 sec 2024-11-06T04:55:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:55:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061665.37 594923.98 57.81 2024-11-06T04:55:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061663.77 594924.23 58.66 lat/lon: 36.70 -121.94 2024-11-06T04:55:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.58 0.21 0.73 0.00 2024-11-06T04:55:20Z- handleDepth()- Depth_Keller msg: 1730868920.11 epoch sec; seqNo:2977; depth 51.47 2024-11-06T04:55:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868920.12 epoch sec; seqNo:2977; 4061720.30 north; 594919.65 east; 36.70 lat; -121.94 lon; 51.47 depth; utmZone: 10 2024-11-06T04:55:20Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:55:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868919.70 2024-11-06T04:55:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:20Z- waiting for 1730868923.70, time is 1730868920.12 2024-11-06T04:55:20Z- AHRS_M2 msg: 1730868920.49 epoch sec; seqNo:2792 2024-11-06T04:55:20Z- AHRS_M2 msg: 0.34 phi; 0.01 theta; 4.07 psi 2024-11-06T04:55:20Z- handleDepth()- Depth_Keller msg: 1730868920.51 epoch sec; seqNo:2978; depth 51.47 2024-11-06T04:55:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868920.52 epoch sec; seqNo:2978; 4061719.91 north; 594918.43 east; 36.70 lat; -121.94 lon; 51.47 depth; utmZone: 10 2024-11-06T04:55:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868919.70 2024-11-06T04:55:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:20Z- waiting for 1730868923.70, time is 1730868920.52 2024-11-06T04:55:20Z- AHRS_M2 msg: 1730868920.90 epoch sec; seqNo:2793 2024-11-06T04:55:20Z- AHRS_M2 msg: 0.34 phi; 0.02 theta; 4.10 psi 2024-11-06T04:55:20Z- handleDepth()- Depth_Keller msg: 1730868920.90 epoch sec; seqNo:2979; depth 51.48 2024-11-06T04:55:20Z- handleDVL() - DVL msg epochMillisec: 1730868920.92 2024-11-06T04:55:20Z- handleDvl() - RDI_Pathfinder msg: 1730868920.92 sec; seqNo:1078; #beams:4 2024-11-06T04:55:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 59.70 , 38.06 , 47.09 , 46.14 2024-11-06T04:55:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , 0.04 , -0.01 2024-11-06T04:55:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868920.94 epoch sec; seqNo:2979; 4061719.91 north; 594918.43 east; 36.70 lat; -121.94 lon; 51.48 depth; utmZone: 10 2024-11-06T04:55:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868919.70 2024-11-06T04:55:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:21Z- waiting for 1730868923.70, time is 1730868920.94 2024-11-06T04:55:21Z- AHRS_M2 msg: 1730868921.31 epoch sec; seqNo:2794 2024-11-06T04:55:21Z- AHRS_M2 msg: 0.33 phi; 0.02 theta; 4.12 psi 2024-11-06T04:55:21Z- handleDepth()- Depth_Keller msg: 1730868921.31 epoch sec; seqNo:2980; depth 51.48 2024-11-06T04:55:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868921.33 epoch sec; seqNo:2980; 4061719.53 north; 594918.44 east; 36.70 lat; -121.94 lon; 51.48 depth; utmZone: 10 2024-11-06T04:55:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868919.70 2024-11-06T04:55:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:21Z- waiting for 1730868923.70, time is 1730868921.33 2024-11-06T04:55:21Z- AHRS_M2 msg: 1730868921.71 epoch sec; seqNo:2795 2024-11-06T04:55:21Z- AHRS_M2 msg: 0.32 phi; 0.03 theta; 4.14 psi 2024-11-06T04:55:21Z- handleDepth()- Depth_Keller msg: 1730868921.71 epoch sec; seqNo:2981; depth 51.48 2024-11-06T04:55:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868921.72 epoch sec; seqNo:2981; 4061719.53 north; 594918.44 east; 36.70 lat; -121.94 lon; 51.48 depth; utmZone: 10 2024-11-06T04:55:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868919.70 2024-11-06T04:55:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:21Z- waiting for 1730868923.70, time is 1730868921.72 2024-11-06T04:55:22Z- AHRS_M2 msg: 1730868922.11 epoch sec; seqNo:2796 2024-11-06T04:55:22Z- AHRS_M2 msg: 0.31 phi; 0.04 theta; 4.15 psi 2024-11-06T04:55:22Z- handleDepth()- Depth_Keller msg: 1730868922.12 epoch sec; seqNo:2982; depth 51.46 2024-11-06T04:55:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868922.13 epoch sec; seqNo:2982; 4061719.14 north; 594917.22 east; 36.70 lat; -121.94 lon; 51.46 depth; utmZone: 10 2024-11-06T04:55:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868919.70 2024-11-06T04:55:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:22Z- waiting for 1730868923.70, time is 1730868922.13 2024-11-06T04:55:22Z- AHRS_M2 msg: 1730868922.52 epoch sec; seqNo:2797 2024-11-06T04:55:22Z- AHRS_M2 msg: 0.30 phi; 0.05 theta; 4.17 psi 2024-11-06T04:55:22Z- handleDepth()- Depth_Keller msg: 1730868922.52 epoch sec; seqNo:2983; depth 51.47 2024-11-06T04:55:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868922.53 epoch sec; seqNo:2983; 4061719.14 north; 594917.22 east; 36.70 lat; -121.94 lon; 51.47 depth; utmZone: 10 2024-11-06T04:55:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868919.70 2024-11-06T04:55:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:22Z- waiting for 1730868923.70, time is 1730868922.53 2024-11-06T04:55:22Z- AHRS_M2 msg: 1730868922.92 epoch sec; seqNo:2798 2024-11-06T04:55:22Z- AHRS_M2 msg: 0.29 phi; 0.05 theta; 4.18 psi 2024-11-06T04:55:22Z- handleDepth()- Depth_Keller msg: 1730868922.93 epoch sec; seqNo:2984; depth 51.47 2024-11-06T04:55:22Z- handleDVL() - DVL msg epochMillisec: 1730868922.95 2024-11-06T04:55:22Z- handleDvl() - RDI_Pathfinder msg: 1730868922.95 sec; seqNo:1079; #beams:4 2024-11-06T04:55:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 56.98 , 38.21 , 47.26 , 44.17 2024-11-06T04:55:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.00 , 0.03 2024-11-06T04:55:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868922.98 epoch sec; seqNo:2984; 4061718.76 north; 594917.22 east; 36.70 lat; -121.94 lon; 51.47 depth; utmZone: 10 2024-11-06T04:55:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868919.70 2024-11-06T04:55:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:23Z- waiting for 1730868923.70, time is 1730868922.98 2024-11-06T04:55:23Z- AHRS_M2 msg: 1730868923.33 epoch sec; seqNo:2799 2024-11-06T04:55:23Z- AHRS_M2 msg: 0.28 phi; 0.06 theta; 4.20 psi 2024-11-06T04:55:23Z- handleDepth()- Depth_Keller msg: 1730868923.34 epoch sec; seqNo:2985; depth 51.44 2024-11-06T04:55:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868923.36 epoch sec; seqNo:2985; 4061718.76 north; 594917.22 east; 36.70 lat; -121.94 lon; 51.44 depth; utmZone: 10 2024-11-06T04:55:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868919.70 2024-11-06T04:55:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:23Z- waiting for 1730868923.70, time is 1730868923.36 2024-11-06T04:55:23Z- AHRS_M2 msg: 1730868923.73 epoch sec; seqNo:2800 2024-11-06T04:55:23Z- AHRS_M2 msg: 0.27 phi; 0.07 theta; 4.21 psi 2024-11-06T04:55:23Z- handleDepth()- Depth_Keller msg: 1730868923.73 epoch sec; seqNo:2986; depth 51.44 2024-11-06T04:55:23Z- handleDVL() - DVL msg epochMillisec: 1730868923.73 2024-11-06T04:55:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:55:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:55:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:55:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:55:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:55:23Z- handleDvl() - RDI_Pathfinder msg: 1730868923.73 sec; seqNo:1080; #beams:0 2024-11-06T04:55:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:55:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.00 , 0.03 2024-11-06T04:55:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868923.76 epoch sec; seqNo:2986; 4061718.75 north; 594916.00 east; 36.70 lat; -121.94 lon; 51.44 depth; utmZone: 10 2024-11-06T04:55:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868919.70 2024-11-06T04:55:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:23Z- waiting for 1730868923.70, time is 1730868923.76 2024-11-06T04:55:23Z- LcmTrn::updateTrn() >>>> pose time 1730868923.76, meas time 1730868923.73 <<<< 2024-11-06T04:55:23Z- LcmTrn::updateTrn() equating pose 1730868923.73 and meas times 1730868923.73 2024-11-06T04:55:23Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868923.73 2024-11-06T04:55:23Z- TerrainNav::motionUpdate() - currEstimate = (4061719, 594916.00, 51.44) 2024-11-06T04:55:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:55:23Z- TerrainNav:: SubcloudNIS is 0.51, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:55:23Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:55:23Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:55:23Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:55:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:55:23Z- variances([0], [2], and psi_berg) = 0.580821 0.208754 0.000000 2024-11-06T04:55:23Z- publishEstimatesDecoder() - at 1730868923.73 sec 2024-11-06T04:55:23Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:55:23Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061663.83 594920.25 58.06 2024-11-06T04:55:23Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061662.21 594920.59 58.62 lat/lon: 36.70 -121.94 2024-11-06T04:55:23Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.58 0.21 0.99 0.00 2024-11-06T04:55:24Z- AHRS_M2 msg: 1730868924.13 epoch sec; seqNo:2801 2024-11-06T04:55:24Z- AHRS_M2 msg: 0.25 phi; 0.08 theta; 4.22 psi 2024-11-06T04:55:24Z- handleDepth()- Depth_Keller msg: 1730868924.14 epoch sec; seqNo:2987; depth 51.44 2024-11-06T04:55:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868924.16 epoch sec; seqNo:2987; 4061718.37 north; 594916.01 east; 36.70 lat; -121.94 lon; 51.44 depth; utmZone: 10 2024-11-06T04:55:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868923.73 2024-11-06T04:55:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:24Z- waiting for 1730868927.73, time is 1730868924.16 2024-11-06T04:55:24Z- AHRS_M2 msg: 1730868924.54 epoch sec; seqNo:2802 2024-11-06T04:55:24Z- AHRS_M2 msg: 0.24 phi; 0.09 theta; 4.24 psi 2024-11-06T04:55:24Z- handleDepth()- Depth_Keller msg: 1730868924.55 epoch sec; seqNo:2988; depth 51.41 2024-11-06T04:55:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868924.56 epoch sec; seqNo:2988; 4061718.37 north; 594916.01 east; 36.70 lat; -121.94 lon; 51.41 depth; utmZone: 10 2024-11-06T04:55:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868923.73 2024-11-06T04:55:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:24Z- waiting for 1730868927.73, time is 1730868924.56 2024-11-06T04:55:24Z- AHRS_M2 msg: 1730868924.95 epoch sec; seqNo:2803 2024-11-06T04:55:24Z- AHRS_M2 msg: 0.23 phi; 0.10 theta; 4.25 psi 2024-11-06T04:55:24Z- handleDepth()- Depth_Keller msg: 1730868924.95 epoch sec; seqNo:2989; depth 51.39 2024-11-06T04:55:24Z- handleDVL() - DVL msg epochMillisec: 1730868924.98 2024-11-06T04:55:24Z- handleDvl() - RDI_Pathfinder msg: 1730868924.98 sec; seqNo:1081; #beams:4 2024-11-06T04:55:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 53.89 , 38.87 , 47.71 , 42.85 2024-11-06T04:55:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , 0.01 2024-11-06T04:55:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868924.99 epoch sec; seqNo:2989; 4061717.99 north; 594916.01 east; 36.70 lat; -121.94 lon; 51.39 depth; utmZone: 10 2024-11-06T04:55:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868923.73 2024-11-06T04:55:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:25Z- waiting for 1730868927.73, time is 1730868924.99 2024-11-06T04:55:25Z- handleDepth()- Depth_Keller msg: 1730868925.40 epoch sec; seqNo:2990; depth 51.37 2024-11-06T04:55:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868925.42 epoch sec; seqNo:2990; 4061717.98 north; 594914.79 east; 36.70 lat; -121.94 lon; 51.37 depth; utmZone: 10 2024-11-06T04:55:25Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:55:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868923.73 2024-11-06T04:55:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:25Z- waiting for 1730868927.73, time is 1730868925.42 2024-11-06T04:55:25Z- AHRS_M2 msg: 1730868925.79 epoch sec; seqNo:2804 2024-11-06T04:55:25Z- AHRS_M2 msg: 0.22 phi; 0.12 theta; 4.26 psi 2024-11-06T04:55:25Z- handleDepth()- Depth_Keller msg: 1730868925.79 epoch sec; seqNo:2991; depth 51.36 2024-11-06T04:55:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868925.80 epoch sec; seqNo:2991; 4061717.60 north; 594914.80 east; 36.70 lat; -121.94 lon; 51.36 depth; utmZone: 10 2024-11-06T04:55:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868923.73 2024-11-06T04:55:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:25Z- waiting for 1730868927.73, time is 1730868925.80 2024-11-06T04:55:26Z- AHRS_M2 msg: 1730868926.19 epoch sec; seqNo:2805 2024-11-06T04:55:26Z- AHRS_M2 msg: 0.21 phi; 0.13 theta; 4.27 psi 2024-11-06T04:55:26Z- handleDepth()- Depth_Keller msg: 1730868926.21 epoch sec; seqNo:2992; depth 51.34 2024-11-06T04:55:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868926.22 epoch sec; seqNo:2992; 4061717.60 north; 594914.80 east; 36.70 lat; -121.94 lon; 51.34 depth; utmZone: 10 2024-11-06T04:55:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868923.73 2024-11-06T04:55:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:26Z- waiting for 1730868927.73, time is 1730868926.22 2024-11-06T04:55:26Z- AHRS_M2 msg: 1730868926.59 epoch sec; seqNo:2806 2024-11-06T04:55:26Z- AHRS_M2 msg: 0.20 phi; 0.14 theta; 4.27 psi 2024-11-06T04:55:26Z- handleDepth()- Depth_Keller msg: 1730868926.60 epoch sec; seqNo:2993; depth 51.30 2024-11-06T04:55:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868926.60 epoch sec; seqNo:2993; 4061717.22 north; 594914.80 east; 36.70 lat; -121.94 lon; 51.30 depth; utmZone: 10 2024-11-06T04:55:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868923.73 2024-11-06T04:55:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:26Z- waiting for 1730868927.73, time is 1730868926.60 2024-11-06T04:55:27Z- AHRS_M2 msg: 1730868927.00 epoch sec; seqNo:2807 2024-11-06T04:55:27Z- AHRS_M2 msg: 0.18 phi; 0.15 theta; 4.27 psi 2024-11-06T04:55:27Z- handleDepth()- Depth_Keller msg: 1730868927.00 epoch sec; seqNo:2994; depth 51.27 2024-11-06T04:55:27Z- handleDVL() - DVL msg epochMillisec: 1730868927.02 2024-11-06T04:55:27Z- handleDvl() - RDI_Pathfinder msg: 1730868927.02 sec; seqNo:1082; #beams:4 2024-11-06T04:55:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 52.78 , 39.53 , 48.59 , 41.96 2024-11-06T04:55:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.04 , -0.02 2024-11-06T04:55:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868927.05 epoch sec; seqNo:2994; 4061717.20 north; 594913.58 east; 36.70 lat; -121.94 lon; 51.27 depth; utmZone: 10 2024-11-06T04:55:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868923.73 2024-11-06T04:55:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:27Z- waiting for 1730868927.73, time is 1730868927.05 2024-11-06T04:55:27Z- AHRS_M2 msg: 1730868927.40 epoch sec; seqNo:2808 2024-11-06T04:55:27Z- AHRS_M2 msg: 0.17 phi; 0.15 theta; 4.27 psi 2024-11-06T04:55:27Z- handleDepth()- Depth_Keller msg: 1730868927.41 epoch sec; seqNo:2995; depth 51.22 2024-11-06T04:55:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868927.42 epoch sec; seqNo:2995; 4061717.20 north; 594913.58 east; 36.70 lat; -121.94 lon; 51.22 depth; utmZone: 10 2024-11-06T04:55:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868923.73 2024-11-06T04:55:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:27Z- waiting for 1730868927.73, time is 1730868927.42 2024-11-06T04:55:27Z- AHRS_M2 msg: 1730868927.81 epoch sec; seqNo:2809 2024-11-06T04:55:27Z- AHRS_M2 msg: 0.16 phi; 0.16 theta; 4.27 psi 2024-11-06T04:55:27Z- handleDepth()- Depth_Keller msg: 1730868927.82 epoch sec; seqNo:2996; depth 51.20 2024-11-06T04:55:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868927.84 epoch sec; seqNo:2996; 4061716.83 north; 594913.58 east; 36.70 lat; -121.94 lon; 51.20 depth; utmZone: 10 2024-11-06T04:55:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868923.73 2024-11-06T04:55:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:27Z- waiting for 1730868927.73, time is 1730868927.84 2024-11-06T04:55:27Z- LcmTrn::updateTrn() >>>> pose time 1730868927.84, meas time 1730868927.02 <<<< 2024-11-06T04:55:27Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868927.84 2024-11-06T04:55:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868927.02 sec, ping # 0; waiting for INS data more recent than 1730868923.73... 2024-11-06T04:55:27Z- TerrainNav::motionUpdate() - currEstimate = (4061717, 594913.58, 51.20) 2024-11-06T04:55:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:55:27Z- TerrainNav:: SubcloudNIS is 0.51, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:55:27Z- TNavPF::Projecting Measurements 2024-11-06T04:55:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:55:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:55:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:55:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:55:27Z- TNavFilter::4 good beams. 2024-11-06T04:55:27Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:55:27Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:55:27Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:55:27Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:55:27Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:55:27Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:55:27Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:55:27Z- TerrainMapOctree: (vehN, vehE ) = (4061717.21, 594914.07). 2024-11-06T04:55:27Z- TerrainMapOctree: Min Distance = 167.31. 2024-11-06T04:55:27Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:55:28Z- Forcing Subcloud Comparison 2024-11-06T04:55:28Z- Weighting particles with subcloud comparison 2024-11-06T04:55:28Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:28Z- meanExpectedMeasDiff: 2.068448 delta_rms_squared: 2.181735 alpha: 0.990916 2024-11-06T04:55:28Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:28Z- meanExpectedMeasDiff: -1.485840 delta_rms_squared: 1.423747 alpha: 0.986147 2024-11-06T04:55:28Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:28Z- meanExpectedMeasDiff: -0.904374 delta_rms_squared: 2.077288 alpha: 0.990464 2024-11-06T04:55:28Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:28Z- meanExpectedMeasDiff: 0.106507 delta_rms_squared: 1.482831 alpha: 0.986692 2024-11-06T04:55:28Z- Subcloud NIS: 0.460217 2024-11-06T04:55:28Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:55:28Z- TNavPF:: sumWeights = 0.330214 2024-11-06T04:55:28Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:55:28Z- TNavPF::First Term of Map Covariance Matrix is 0.99 2024-11-06T04:55:28Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T04:55:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868927.02 sec. 2024-11-06T04:55:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:55:28Z- variances([0], [2], and psi_berg) = 0.577624 0.211345 0.000000 2024-11-06T04:55:28Z- publishEstimatesDecoder() - at 1730868927.84 sec 2024-11-06T04:55:28Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:55:28Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061660.65 594918.10 57.98 2024-11-06T04:55:28Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061660.29 594918.17 58.45 lat/lon: 36.70 -121.94 2024-11-06T04:55:28Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.58 0.21 0.78 0.00 2024-11-06T04:55:28Z- AHRS_M2 msg: 1730868928.21 epoch sec; seqNo:2810 2024-11-06T04:55:28Z- AHRS_M2 msg: 0.15 phi; 0.17 theta; 4.27 psi 2024-11-06T04:55:28Z- handleDepth()- Depth_Keller msg: 1730868928.21 epoch sec; seqNo:2997; depth 51.14 2024-11-06T04:55:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868928.22 epoch sec; seqNo:2997; 4061716.83 north; 594913.58 east; 36.70 lat; -121.94 lon; 51.14 depth; utmZone: 10 2024-11-06T04:55:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868927.84 2024-11-06T04:55:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:28Z- waiting for 1730868931.84, time is 1730868928.22 2024-11-06T04:55:28Z- AHRS_M2 msg: 1730868928.62 epoch sec; seqNo:2811 2024-11-06T04:55:28Z- AHRS_M2 msg: 0.14 phi; 0.17 theta; 4.27 psi 2024-11-06T04:55:28Z- handleDepth()- Depth_Keller msg: 1730868928.63 epoch sec; seqNo:2998; depth 51.11 2024-11-06T04:55:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868928.63 epoch sec; seqNo:2998; 4061716.43 north; 594912.37 east; 36.70 lat; -121.94 lon; 51.11 depth; utmZone: 10 2024-11-06T04:55:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868927.84 2024-11-06T04:55:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:28Z- waiting for 1730868931.84, time is 1730868928.63 2024-11-06T04:55:29Z- AHRS_M2 msg: 1730868929.02 epoch sec; seqNo:2812 2024-11-06T04:55:29Z- AHRS_M2 msg: 0.13 phi; 0.18 theta; 4.26 psi 2024-11-06T04:55:29Z- handleDepth()- Depth_Keller msg: 1730868929.02 epoch sec; seqNo:2999; depth 51.06 2024-11-06T04:55:29Z- handleDVL() - DVL msg epochMillisec: 1730868929.05 2024-11-06T04:55:29Z- handleDvl() - RDI_Pathfinder msg: 1730868929.05 sec; seqNo:1083; #beams:4 2024-11-06T04:55:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.80 , 40.64 , 49.25 , 41.08 2024-11-06T04:55:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , -0.03 2024-11-06T04:55:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868929.07 epoch sec; seqNo:2999; 4061716.43 north; 594912.37 east; 36.70 lat; -121.94 lon; 51.06 depth; utmZone: 10 2024-11-06T04:55:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868927.84 2024-11-06T04:55:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:29Z- waiting for 1730868931.84, time is 1730868929.07 2024-11-06T04:55:29Z- AHRS_M2 msg: 1730868929.44 epoch sec; seqNo:2813 2024-11-06T04:55:29Z- AHRS_M2 msg: 0.13 phi; 0.18 theta; 4.26 psi 2024-11-06T04:55:29Z- handleDepth()- Depth_Keller msg: 1730868929.44 epoch sec; seqNo:3000; depth 51.00 2024-11-06T04:55:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868929.46 epoch sec; seqNo:3000; 4061716.05 north; 594912.37 east; 36.70 lat; -121.94 lon; 51.00 depth; utmZone: 10 2024-11-06T04:55:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868927.84 2024-11-06T04:55:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:29Z- waiting for 1730868931.84, time is 1730868929.46 2024-11-06T04:55:29Z- AHRS_M2 msg: 1730868929.84 epoch sec; seqNo:2814 2024-11-06T04:55:29Z- AHRS_M2 msg: 0.12 phi; 0.19 theta; 4.25 psi 2024-11-06T04:55:29Z- handleDepth()- Depth_Keller msg: 1730868929.85 epoch sec; seqNo:3001; depth 50.96 2024-11-06T04:55:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868929.86 epoch sec; seqNo:3001; 4061716.05 north; 594912.37 east; 36.70 lat; -121.94 lon; 50.96 depth; utmZone: 10 2024-11-06T04:55:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868927.84 2024-11-06T04:55:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:30Z- waiting for 1730868931.84, time is 1730868929.86 2024-11-06T04:55:30Z- AHRS_M2 msg: 1730868930.24 epoch sec; seqNo:2815 2024-11-06T04:55:30Z- AHRS_M2 msg: 0.11 phi; 0.19 theta; 4.24 psi 2024-11-06T04:55:30Z- handleDepth()- Depth_Keller msg: 1730868930.25 epoch sec; seqNo:3002; depth 50.91 2024-11-06T04:55:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868930.26 epoch sec; seqNo:3002; 4061716.04 north; 594911.15 east; 36.70 lat; -121.94 lon; 50.91 depth; utmZone: 10 2024-11-06T04:55:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868927.84 2024-11-06T04:55:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:30Z- waiting for 1730868931.84, time is 1730868930.26 2024-11-06T04:55:30Z- AHRS_M2 msg: 1730868930.66 epoch sec; seqNo:2816 2024-11-06T04:55:30Z- AHRS_M2 msg: 0.10 phi; 0.19 theta; 4.24 psi 2024-11-06T04:55:30Z- handleDepth()- Depth_Keller msg: 1730868930.67 epoch sec; seqNo:3003; depth 50.85 2024-11-06T04:55:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868930.70 epoch sec; seqNo:3003; 4061715.66 north; 594911.16 east; 36.70 lat; -121.94 lon; 50.85 depth; utmZone: 10 2024-11-06T04:55:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868927.84 2024-11-06T04:55:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:30Z- waiting for 1730868931.84, time is 1730868930.70 2024-11-06T04:55:31Z- AHRS_M2 msg: 1730868931.08 epoch sec; seqNo:2817 2024-11-06T04:55:31Z- AHRS_M2 msg: 0.10 phi; 0.20 theta; 4.23 psi 2024-11-06T04:55:31Z- handleDepth()- Depth_Keller msg: 1730868931.09 epoch sec; seqNo:3004; depth 50.79 2024-11-06T04:55:31Z- handleDVL() - DVL msg epochMillisec: 1730868931.11 2024-11-06T04:55:31Z- handleDvl() - RDI_Pathfinder msg: 1730868931.11 sec; seqNo:1084; #beams:4 2024-11-06T04:55:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.47 , 41.52 , 49.69 , 40.64 2024-11-06T04:55:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , -0.02 2024-11-06T04:55:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868931.14 epoch sec; seqNo:3004; 4061715.66 north; 594911.16 east; 36.70 lat; -121.94 lon; 50.79 depth; utmZone: 10 2024-11-06T04:55:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868927.84 2024-11-06T04:55:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:31Z- waiting for 1730868931.84, time is 1730868931.14 2024-11-06T04:55:31Z- AHRS_M2 msg: 1730868931.45 epoch sec; seqNo:2818 2024-11-06T04:55:31Z- AHRS_M2 msg: 0.09 phi; 0.20 theta; 4.23 psi 2024-11-06T04:55:31Z- handleDepth()- Depth_Keller msg: 1730868931.46 epoch sec; seqNo:3005; depth 50.71 2024-11-06T04:55:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868931.48 epoch sec; seqNo:3005; 4061715.28 north; 594911.16 east; 36.70 lat; -121.94 lon; 50.71 depth; utmZone: 10 2024-11-06T04:55:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868927.84 2024-11-06T04:55:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:31Z- waiting for 1730868931.84, time is 1730868931.48 2024-11-06T04:55:31Z- AHRS_M2 msg: 1730868931.86 epoch sec; seqNo:2819 2024-11-06T04:55:31Z- AHRS_M2 msg: 0.09 phi; 0.20 theta; 4.22 psi 2024-11-06T04:55:31Z- handleDepth()- Depth_Keller msg: 1730868931.87 epoch sec; seqNo:3006; depth 50.66 2024-11-06T04:55:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868931.88 epoch sec; seqNo:3006; 4061715.27 north; 594909.94 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T04:55:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868927.84 2024-11-06T04:55:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:32Z- waiting for 1730868931.84, time is 1730868931.88 2024-11-06T04:55:32Z- LcmTrn::updateTrn() >>>> pose time 1730868931.88, meas time 1730868931.11 <<<< 2024-11-06T04:55:32Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868931.88 2024-11-06T04:55:32Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868931.11 sec, ping # 0; waiting for INS data more recent than 1730868927.84... 2024-11-06T04:55:32Z- TerrainNav::motionUpdate() - currEstimate = (4061715, 594909.94, 50.66) 2024-11-06T04:55:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:55:32Z- TerrainNav:: SubcloudNIS is 0.46, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:55:32Z- TNavPF::Projecting Measurements 2024-11-06T04:55:32Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:55:32Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:55:32Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:55:32Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:55:32Z- TNavFilter::4 good beams. 2024-11-06T04:55:32Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:55:32Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:55:32Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:55:32Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:55:32Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:55:32Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:55:32Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:55:32Z- TerrainMapOctree: (vehN, vehE ) = (4061715.56, 594910.63). 2024-11-06T04:55:32Z- TerrainMapOctree: Min Distance = 164.28. 2024-11-06T04:55:32Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:55:32Z- Forcing Subcloud Comparison 2024-11-06T04:55:32Z- Weighting particles with subcloud comparison 2024-11-06T04:55:32Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:32Z- meanExpectedMeasDiff: 0.956258 delta_rms_squared: 1.620869 alpha: 0.987811 2024-11-06T04:55:32Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:32Z- meanExpectedMeasDiff: -1.701250 delta_rms_squared: 1.284454 alpha: 0.984668 2024-11-06T04:55:32Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:32Z- meanExpectedMeasDiff: -1.867249 delta_rms_squared: 1.831980 alpha: 0.989201 2024-11-06T04:55:32Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:32Z- meanExpectedMeasDiff: -0.425093 delta_rms_squared: 1.158299 alpha: 0.983026 2024-11-06T04:55:32Z- Subcloud NIS: 0.425065 2024-11-06T04:55:32Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:55:32Z- TNavPF:: sumWeights = 0.377758 2024-11-06T04:55:32Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:55:32Z- TNavPF::First Term of Map Covariance Matrix is 0.57 2024-11-06T04:55:32Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.05 2024-11-06T04:55:32Z- TerrainNav::Resampling particle filter... 2024-11-06T04:55:32Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868931.11 sec. 2024-11-06T04:55:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:55:32Z- variances([0], [2], and psi_berg) = 0.572685 0.214433 0.000000 2024-11-06T04:55:32Z- publishEstimatesDecoder() - at 1730868931.88 sec 2024-11-06T04:55:32Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:55:32Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061660.45 594914.13 56.16 2024-11-06T04:55:32Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061658.74 594914.52 58.27 lat/lon: 36.70 -121.94 2024-11-06T04:55:32Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.57 0.21 0.73 0.00 2024-11-06T04:55:32Z- AHRS_M2 msg: 1730868932.28 epoch sec; seqNo:2820 2024-11-06T04:55:32Z- AHRS_M2 msg: 0.08 phi; 0.20 theta; 4.21 psi 2024-11-06T04:55:32Z- handleDepth()- Depth_Keller msg: 1730868932.29 epoch sec; seqNo:3007; depth 50.57 2024-11-06T04:55:32Z- handleDVL() - DVL msg epochMillisec: 1730868932.30 2024-11-06T04:55:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:55:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:55:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:55:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:55:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:55:32Z- handleDvl() - RDI_Pathfinder msg: 1730868932.30 sec; seqNo:1085; #beams:0 2024-11-06T04:55:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:55:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.00 , -0.02 2024-11-06T04:55:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868932.32 epoch sec; seqNo:3007; 4061715.27 north; 594909.94 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T04:55:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868931.88 2024-11-06T04:55:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:32Z- waiting for 1730868935.88, time is 1730868932.32 2024-11-06T04:55:32Z- AHRS_M2 msg: 1730868932.67 epoch sec; seqNo:2821 2024-11-06T04:55:32Z- AHRS_M2 msg: 0.07 phi; 0.20 theta; 4.21 psi 2024-11-06T04:55:32Z- handleDepth()- Depth_Keller msg: 1730868932.68 epoch sec; seqNo:3008; depth 50.53 2024-11-06T04:55:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868932.74 epoch sec; seqNo:3008; 4061714.89 north; 594909.94 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T04:55:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868931.88 2024-11-06T04:55:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:32Z- waiting for 1730868935.88, time is 1730868932.74 2024-11-06T04:55:33Z- AHRS_M2 msg: 1730868933.07 epoch sec; seqNo:2822 2024-11-06T04:55:33Z- AHRS_M2 msg: 0.07 phi; 0.20 theta; 4.20 psi 2024-11-06T04:55:33Z- handleDepth()- Depth_Keller msg: 1730868933.08 epoch sec; seqNo:3009; depth 50.46 2024-11-06T04:55:33Z- handleDVL() - DVL msg epochMillisec: 1730868933.10 2024-11-06T04:55:33Z- handleDvl() - RDI_Pathfinder msg: 1730868933.10 sec; seqNo:1086; #beams:4 2024-11-06T04:55:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.81 , 42.40 , 49.69 , 40.64 2024-11-06T04:55:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , -0.02 2024-11-06T04:55:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868933.13 epoch sec; seqNo:3009; 4061714.89 north; 594909.94 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T04:55:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868931.88 2024-11-06T04:55:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:33Z- waiting for 1730868935.88, time is 1730868933.13 2024-11-06T04:55:33Z- AHRS_M2 msg: 1730868933.47 epoch sec; seqNo:2823 2024-11-06T04:55:33Z- AHRS_M2 msg: 0.06 phi; 0.20 theta; 4.19 psi 2024-11-06T04:55:33Z- handleDepth()- Depth_Keller msg: 1730868933.49 epoch sec; seqNo:3010; depth 50.38 2024-11-06T04:55:33Z- handleDVL() - DVL msg epochMillisec: 1730868933.49 2024-11-06T04:55:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:55:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:55:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:55:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:55:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:55:33Z- handleDvl() - RDI_Pathfinder msg: 1730868933.49 sec; seqNo:1087; #beams:0 2024-11-06T04:55:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:55:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.00 , -0.02 2024-11-06T04:55:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868933.51 epoch sec; seqNo:3010; 4061714.50 north; 594908.73 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:55:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868931.88 2024-11-06T04:55:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:33Z- waiting for 1730868935.88, time is 1730868933.51 2024-11-06T04:55:33Z- AHRS_M2 msg: 1730868933.88 epoch sec; seqNo:2824 2024-11-06T04:55:33Z- AHRS_M2 msg: 0.05 phi; 0.20 theta; 4.18 psi 2024-11-06T04:55:33Z- handleDepth()- Depth_Keller msg: 1730868933.89 epoch sec; seqNo:3011; depth 50.30 2024-11-06T04:55:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868933.91 epoch sec; seqNo:3011; 4061714.50 north; 594908.73 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:55:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868931.88 2024-11-06T04:55:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:34Z- waiting for 1730868935.88, time is 1730868933.91 2024-11-06T04:55:34Z- AHRS_M2 msg: 1730868934.28 epoch sec; seqNo:2825 2024-11-06T04:55:34Z- AHRS_M2 msg: 0.05 phi; 0.20 theta; 4.17 psi 2024-11-06T04:55:34Z- handleDepth()- Depth_Keller msg: 1730868934.29 epoch sec; seqNo:3012; depth 50.25 2024-11-06T04:55:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868934.30 epoch sec; seqNo:3012; 4061714.12 north; 594908.73 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T04:55:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868931.88 2024-11-06T04:55:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:34Z- waiting for 1730868935.88, time is 1730868934.30 2024-11-06T04:55:34Z- AHRS_M2 msg: 1730868934.72 epoch sec; seqNo:2826 2024-11-06T04:55:34Z- AHRS_M2 msg: 0.05 phi; 0.20 theta; 4.16 psi 2024-11-06T04:55:34Z- handleDepth()- Depth_Keller msg: 1730868934.75 epoch sec; seqNo:3013; depth 50.17 2024-11-06T04:55:34Z- handleDVL() - DVL msg epochMillisec: 1730868934.78 2024-11-06T04:55:34Z- handleDvl() - RDI_Pathfinder msg: 1730868934.78 sec; seqNo:1088; #beams:4 2024-11-06T04:55:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.15 , 43.07 , 49.25 , 40.64 2024-11-06T04:55:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.00 , -0.02 2024-11-06T04:55:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868934.81 epoch sec; seqNo:3013; 4061714.12 north; 594908.73 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T04:55:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868931.88 2024-11-06T04:55:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:34Z- waiting for 1730868935.88, time is 1730868934.81 2024-11-06T04:55:35Z- handleDepth()- Depth_Keller msg: 1730868935.11 epoch sec; seqNo:3014; depth 50.08 2024-11-06T04:55:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868935.12 epoch sec; seqNo:3014; 4061713.73 north; 594907.52 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T04:55:35Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:55:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868931.88 2024-11-06T04:55:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:35Z- waiting for 1730868935.88, time is 1730868935.12 2024-11-06T04:55:35Z- AHRS_M2 msg: 1730868935.49 epoch sec; seqNo:2827 2024-11-06T04:55:35Z- AHRS_M2 msg: 0.04 phi; 0.19 theta; 4.14 psi 2024-11-06T04:55:35Z- handleDepth()- Depth_Keller msg: 1730868935.50 epoch sec; seqNo:3015; depth 50.02 2024-11-06T04:55:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868935.51 epoch sec; seqNo:3015; 4061713.73 north; 594907.52 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:55:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868931.88 2024-11-06T04:55:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:35Z- waiting for 1730868935.88, time is 1730868935.51 2024-11-06T04:55:35Z- AHRS_M2 msg: 1730868935.91 epoch sec; seqNo:2828 2024-11-06T04:55:35Z- AHRS_M2 msg: 0.04 phi; 0.19 theta; 4.13 psi 2024-11-06T04:55:35Z- handleDepth()- Depth_Keller msg: 1730868935.91 epoch sec; seqNo:3016; depth 49.96 2024-11-06T04:55:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868935.92 epoch sec; seqNo:3016; 4061713.35 north; 594907.52 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:55:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868931.88 2024-11-06T04:55:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:36Z- waiting for 1730868935.88, time is 1730868935.92 2024-11-06T04:55:36Z- LcmTrn::updateTrn() >>>> pose time 1730868935.92, meas time 1730868934.78 <<<< 2024-11-06T04:55:36Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868935.92 2024-11-06T04:55:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868934.78 sec, ping # 0; waiting for INS data more recent than 1730868931.88... 2024-11-06T04:55:36Z- TerrainNav::motionUpdate() - currEstimate = (4061713, 594907.52, 49.96) 2024-11-06T04:55:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:55:36Z- TerrainNav:: SubcloudNIS is 0.43, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:55:36Z- TNavPF::Projecting Measurements 2024-11-06T04:55:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:55:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:55:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:55:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:55:36Z- TNavFilter::4 good beams. 2024-11-06T04:55:36Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:55:36Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:55:36Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:55:36Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:55:36Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:55:36Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:55:36Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:55:36Z- TerrainMapOctree: (vehN, vehE ) = (4061713.89, 594908.20). 2024-11-06T04:55:36Z- TerrainMapOctree: Min Distance = 162.27. 2024-11-06T04:55:36Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:55:36Z- Forcing Subcloud Comparison 2024-11-06T04:55:36Z- Weighting particles with subcloud comparison 2024-11-06T04:55:36Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:36Z- meanExpectedMeasDiff: 0.825409 delta_rms_squared: 1.423176 alpha: 0.986142 2024-11-06T04:55:36Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:36Z- meanExpectedMeasDiff: -1.383334 delta_rms_squared: 1.254589 alpha: 0.984309 2024-11-06T04:55:36Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:36Z- meanExpectedMeasDiff: -2.359699 delta_rms_squared: 1.694820 alpha: 0.988337 2024-11-06T04:55:36Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:36Z- meanExpectedMeasDiff: -0.593923 delta_rms_squared: 1.078041 alpha: 0.981786 2024-11-06T04:55:36Z- Subcloud NIS: 0.386045 2024-11-06T04:55:36Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:55:36Z- TNavPF:: sumWeights = 0.373215 2024-11-06T04:55:36Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:55:36Z- TNavPF::First Term of Map Covariance Matrix is 0.51 2024-11-06T04:55:36Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.04 2024-11-06T04:55:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868934.78 sec. 2024-11-06T04:55:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:55:36Z- variances([0], [2], and psi_berg) = 0.577379 0.216934 0.000000 2024-11-06T04:55:36Z- publishEstimatesDecoder() - at 1730868935.92 sec 2024-11-06T04:55:36Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:55:36Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061657.49 594912.46 57.05 2024-11-06T04:55:36Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061656.82 594912.09 57.97 lat/lon: 36.70 -121.94 2024-11-06T04:55:36Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.58 0.22 0.71 0.00 2024-11-06T04:55:36Z- AHRS_M2 msg: 1730868936.30 epoch sec; seqNo:2829 2024-11-06T04:55:36Z- AHRS_M2 msg: 0.04 phi; 0.18 theta; 4.12 psi 2024-11-06T04:55:36Z- handleDepth()- Depth_Keller msg: 1730868936.32 epoch sec; seqNo:3017; depth 49.88 2024-11-06T04:55:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868936.32 epoch sec; seqNo:3017; 4061713.35 north; 594907.52 east; 36.70 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T04:55:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868935.92 2024-11-06T04:55:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:36Z- waiting for 1730868939.92, time is 1730868936.32 2024-11-06T04:55:36Z- AHRS_M2 msg: 1730868936.75 epoch sec; seqNo:2830 2024-11-06T04:55:36Z- AHRS_M2 msg: 0.04 phi; 0.18 theta; 4.11 psi 2024-11-06T04:55:36Z- handleDepth()- Depth_Keller msg: 1730868936.75 epoch sec; seqNo:3018; depth 49.81 2024-11-06T04:55:36Z- handleDVL() - DVL msg epochMillisec: 1730868936.77 2024-11-06T04:55:36Z- handleDvl() - RDI_Pathfinder msg: 1730868936.77 sec; seqNo:1089; #beams:4 2024-11-06T04:55:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.11 , 43.42 , 49.13 , 41.18 2024-11-06T04:55:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.00 , -0.01 2024-11-06T04:55:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868936.80 epoch sec; seqNo:3018; 4061712.96 north; 594906.30 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:55:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868935.92 2024-11-06T04:55:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:36Z- waiting for 1730868939.92, time is 1730868936.80 2024-11-06T04:55:37Z- handleDepth()- Depth_Keller msg: 1730868937.13 epoch sec; seqNo:3019; depth 49.72 2024-11-06T04:55:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868937.15 epoch sec; seqNo:3019; 4061712.96 north; 594906.30 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:55:37Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:55:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868935.92 2024-11-06T04:55:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:37Z- waiting for 1730868939.92, time is 1730868937.15 2024-11-06T04:55:37Z- AHRS_M2 msg: 1730868937.52 epoch sec; seqNo:2831 2024-11-06T04:55:37Z- AHRS_M2 msg: 0.05 phi; 0.17 theta; 4.09 psi 2024-11-06T04:55:37Z- handleDepth()- Depth_Keller msg: 1730868937.52 epoch sec; seqNo:3020; depth 49.67 2024-11-06T04:55:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868937.53 epoch sec; seqNo:3020; 4061712.58 north; 594906.31 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:55:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868935.92 2024-11-06T04:55:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:37Z- waiting for 1730868939.92, time is 1730868937.53 2024-11-06T04:55:37Z- AHRS_M2 msg: 1730868937.92 epoch sec; seqNo:2832 2024-11-06T04:55:37Z- AHRS_M2 msg: 0.05 phi; 0.17 theta; 4.08 psi 2024-11-06T04:55:37Z- handleDepth()- Depth_Keller msg: 1730868937.93 epoch sec; seqNo:3021; depth 49.60 2024-11-06T04:55:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868937.95 epoch sec; seqNo:3021; 4061712.58 north; 594906.31 east; 36.70 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T04:55:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868935.92 2024-11-06T04:55:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:38Z- waiting for 1730868939.92, time is 1730868937.95 2024-11-06T04:55:38Z- AHRS_M2 msg: 1730868938.32 epoch sec; seqNo:2833 2024-11-06T04:55:38Z- AHRS_M2 msg: 0.04 phi; 0.16 theta; 4.07 psi 2024-11-06T04:55:38Z- handleDepth()- Depth_Keller msg: 1730868938.33 epoch sec; seqNo:3022; depth 49.55 2024-11-06T04:55:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868938.34 epoch sec; seqNo:3022; 4061712.18 north; 594905.09 east; 36.70 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T04:55:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868935.92 2024-11-06T04:55:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:38Z- waiting for 1730868939.92, time is 1730868938.34 2024-11-06T04:55:38Z- AHRS_M2 msg: 1730868938.77 epoch sec; seqNo:2834 2024-11-06T04:55:38Z- AHRS_M2 msg: 0.03 phi; 0.15 theta; 4.05 psi 2024-11-06T04:55:38Z- handleDepth()- Depth_Keller msg: 1730868938.78 epoch sec; seqNo:3023; depth 49.47 2024-11-06T04:55:38Z- handleDVL() - DVL msg epochMillisec: 1730868938.80 2024-11-06T04:55:38Z- handleDvl() - RDI_Pathfinder msg: 1730868938.80 sec; seqNo:1090; #beams:4 2024-11-06T04:55:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.15 , 43.51 , 48.81 , 41.96 2024-11-06T04:55:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , -0.01 2024-11-06T04:55:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868938.84 epoch sec; seqNo:3023; 4061711.81 north; 594905.10 east; 36.70 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T04:55:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868935.92 2024-11-06T04:55:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:38Z- waiting for 1730868939.92, time is 1730868938.84 2024-11-06T04:55:39Z- handleDepth()- Depth_Keller msg: 1730868939.17 epoch sec; seqNo:3024; depth 49.39 2024-11-06T04:55:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868939.19 epoch sec; seqNo:3024; 4061711.81 north; 594905.10 east; 36.70 lat; -121.94 lon; 49.39 depth; utmZone: 10 2024-11-06T04:55:39Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:55:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868935.92 2024-11-06T04:55:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:39Z- waiting for 1730868939.92, time is 1730868939.19 2024-11-06T04:55:39Z- AHRS_M2 msg: 1730868939.57 epoch sec; seqNo:2835 2024-11-06T04:55:39Z- AHRS_M2 msg: 0.02 phi; 0.14 theta; 4.02 psi 2024-11-06T04:55:39Z- handleDepth()- Depth_Keller msg: 1730868939.58 epoch sec; seqNo:3025; depth 49.34 2024-11-06T04:55:39Z- handleDVL() - DVL msg epochMillisec: 1730868939.58 2024-11-06T04:55:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:55:39Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:55:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:55:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:55:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:55:39Z- handleDvl() - RDI_Pathfinder msg: 1730868939.58 sec; seqNo:1091; #beams:0 2024-11-06T04:55:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:55:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.01 , -0.01 2024-11-06T04:55:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868939.59 epoch sec; seqNo:3025; 4061711.43 north; 594905.10 east; 36.70 lat; -121.94 lon; 49.34 depth; utmZone: 10 2024-11-06T04:55:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868935.92 2024-11-06T04:55:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:39Z- waiting for 1730868939.92, time is 1730868939.59 2024-11-06T04:55:39Z- AHRS_M2 msg: 1730868939.98 epoch sec; seqNo:2836 2024-11-06T04:55:39Z- AHRS_M2 msg: 0.02 phi; 0.13 theta; 4.01 psi 2024-11-06T04:55:39Z- handleDepth()- Depth_Keller msg: 1730868939.98 epoch sec; seqNo:3026; depth 49.28 2024-11-06T04:55:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868939.99 epoch sec; seqNo:3026; 4061711.05 north; 594905.10 east; 36.70 lat; -121.94 lon; 49.28 depth; utmZone: 10 2024-11-06T04:55:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868935.92 2024-11-06T04:55:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:40Z- waiting for 1730868939.92, time is 1730868939.99 2024-11-06T04:55:40Z- LcmTrn::updateTrn() >>>> pose time 1730868939.99, meas time 1730868939.58 <<<< 2024-11-06T04:55:40Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868939.99 2024-11-06T04:55:40Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:55:40Z- TerrainNav::motionUpdate() - currEstimate = (4061711, 594905.10, 49.28) 2024-11-06T04:55:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:55:40Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:55:40Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:55:40Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:55:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:55:40Z- variances([0], [2], and psi_berg) = 0.579527 0.218316 0.000000 2024-11-06T04:55:40Z- publishEstimatesDecoder() - at 1730868939.99 sec 2024-11-06T04:55:40Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:55:40Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061655.25 594910.02 56.44 2024-11-06T04:55:40Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061654.52 594909.68 57.29 lat/lon: 36.70 -121.94 2024-11-06T04:55:40Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.58 0.22 0.96 0.00 2024-11-06T04:55:40Z- AHRS_M2 msg: 1730868940.37 epoch sec; seqNo:2837 2024-11-06T04:55:40Z- AHRS_M2 msg: 0.02 phi; 0.13 theta; 4.00 psi 2024-11-06T04:55:40Z- handleDepth()- Depth_Keller msg: 1730868940.39 epoch sec; seqNo:3027; depth 49.22 2024-11-06T04:55:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868940.41 epoch sec; seqNo:3027; 4061711.03 north; 594903.88 east; 36.70 lat; -121.94 lon; 49.22 depth; utmZone: 10 2024-11-06T04:55:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868939.99 2024-11-06T04:55:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:40Z- waiting for 1730868943.99, time is 1730868940.41 2024-11-06T04:55:40Z- handleDepth()- Depth_Keller msg: 1730868940.79 epoch sec; seqNo:3028; depth 49.16 2024-11-06T04:55:40Z- handleDVL() - DVL msg epochMillisec: 1730868940.83 2024-11-06T04:55:40Z- handleDvl() - RDI_Pathfinder msg: 1730868940.83 sec; seqNo:1092; #beams:4 2024-11-06T04:55:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.48 , 43.73 , 48.37 , 42.40 2024-11-06T04:55:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , -0.03 2024-11-06T04:55:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868940.84 epoch sec; seqNo:3028; 4061710.66 north; 594903.89 east; 36.70 lat; -121.94 lon; 49.16 depth; utmZone: 10 2024-11-06T04:55:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868939.99 2024-11-06T04:55:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:40Z- waiting for 1730868943.99, time is 1730868940.84 2024-11-06T04:55:41Z- AHRS_M2 msg: 1730868941.18 epoch sec; seqNo:2838 2024-11-06T04:55:41Z- AHRS_M2 msg: 0.02 phi; 0.11 theta; 3.97 psi 2024-11-06T04:55:41Z- handleDepth()- Depth_Keller msg: 1730868941.19 epoch sec; seqNo:3029; depth 49.10 2024-11-06T04:55:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868941.20 epoch sec; seqNo:3029; 4061710.66 north; 594903.89 east; 36.70 lat; -121.94 lon; 49.10 depth; utmZone: 10 2024-11-06T04:55:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868939.99 2024-11-06T04:55:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:41Z- waiting for 1730868943.99, time is 1730868941.20 2024-11-06T04:55:41Z- AHRS_M2 msg: 1730868941.58 epoch sec; seqNo:2839 2024-11-06T04:55:41Z- AHRS_M2 msg: 0.02 phi; 0.10 theta; 3.96 psi 2024-11-06T04:55:41Z- handleDepth()- Depth_Keller msg: 1730868941.59 epoch sec; seqNo:3030; depth 49.06 2024-11-06T04:55:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868941.60 epoch sec; seqNo:3030; 4061710.28 north; 594903.89 east; 36.70 lat; -121.94 lon; 49.06 depth; utmZone: 10 2024-11-06T04:55:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868939.99 2024-11-06T04:55:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:41Z- waiting for 1730868943.99, time is 1730868941.60 2024-11-06T04:55:41Z- AHRS_M2 msg: 1730868941.99 epoch sec; seqNo:2840 2024-11-06T04:55:41Z- AHRS_M2 msg: 0.02 phi; 0.10 theta; 3.95 psi 2024-11-06T04:55:41Z- handleDepth()- Depth_Keller msg: 1730868941.99 epoch sec; seqNo:3031; depth 49.01 2024-11-06T04:55:42Z- handleDVL() - DVL msg epochMillisec: 1730868942.00 2024-11-06T04:55:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:55:42Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:55:42Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:55:42Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:55:42Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:55:42Z- handleDvl() - RDI_Pathfinder msg: 1730868942.00 sec; seqNo:1093; #beams:0 2024-11-06T04:55:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:55:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , -0.03 2024-11-06T04:55:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868942.02 epoch sec; seqNo:3031; 4061709.88 north; 594902.68 east; 36.70 lat; -121.94 lon; 49.01 depth; utmZone: 10 2024-11-06T04:55:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868939.99 2024-11-06T04:55:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:42Z- waiting for 1730868943.99, time is 1730868942.02 2024-11-06T04:55:42Z- AHRS_M2 msg: 1730868942.40 epoch sec; seqNo:2841 2024-11-06T04:55:42Z- AHRS_M2 msg: 0.03 phi; 0.09 theta; 3.94 psi 2024-11-06T04:55:42Z- handleDepth()- Depth_Keller msg: 1730868942.40 epoch sec; seqNo:3032; depth 48.96 2024-11-06T04:55:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868942.41 epoch sec; seqNo:3032; 4061709.88 north; 594902.68 east; 36.70 lat; -121.94 lon; 48.96 depth; utmZone: 10 2024-11-06T04:55:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868939.99 2024-11-06T04:55:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:42Z- waiting for 1730868943.99, time is 1730868942.41 2024-11-06T04:55:42Z- AHRS_M2 msg: 1730868942.79 epoch sec; seqNo:2842 2024-11-06T04:55:42Z- AHRS_M2 msg: 0.03 phi; 0.08 theta; 3.93 psi 2024-11-06T04:55:42Z- handleDepth()- Depth_Keller msg: 1730868942.81 epoch sec; seqNo:3033; depth 48.91 2024-11-06T04:55:42Z- handleDVL() - DVL msg epochMillisec: 1730868942.84 2024-11-06T04:55:42Z- handleDvl() - RDI_Pathfinder msg: 1730868942.84 sec; seqNo:1094; #beams:4 2024-11-06T04:55:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 43.73 , 47.92 , 43.07 2024-11-06T04:55:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.07 , -0.02 2024-11-06T04:55:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868942.85 epoch sec; seqNo:3033; 4061709.51 north; 594902.68 east; 36.70 lat; -121.94 lon; 48.91 depth; utmZone: 10 2024-11-06T04:55:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868939.99 2024-11-06T04:55:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:42Z- waiting for 1730868943.99, time is 1730868942.85 2024-11-06T04:55:43Z- AHRS_M2 msg: 1730868943.20 epoch sec; seqNo:2843 2024-11-06T04:55:43Z- AHRS_M2 msg: 0.03 phi; 0.08 theta; 3.91 psi 2024-11-06T04:55:43Z- handleDepth()- Depth_Keller msg: 1730868943.21 epoch sec; seqNo:3034; depth 48.88 2024-11-06T04:55:43Z- handleDVL() - DVL msg epochMillisec: 1730868943.21 2024-11-06T04:55:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:55:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:55:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:55:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:55:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:55:43Z- handleDvl() - RDI_Pathfinder msg: 1730868943.21 sec; seqNo:1095; #beams:0 2024-11-06T04:55:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:55:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.07 , -0.02 2024-11-06T04:55:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868943.25 epoch sec; seqNo:3034; 4061709.13 north; 594902.69 east; 36.70 lat; -121.94 lon; 48.88 depth; utmZone: 10 2024-11-06T04:55:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868939.99 2024-11-06T04:55:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:43Z- waiting for 1730868943.99, time is 1730868943.25 2024-11-06T04:55:43Z- AHRS_M2 msg: 1730868943.60 epoch sec; seqNo:2844 2024-11-06T04:55:43Z- AHRS_M2 msg: 0.03 phi; 0.07 theta; 3.90 psi 2024-11-06T04:55:43Z- handleDepth()- Depth_Keller msg: 1730868943.61 epoch sec; seqNo:3035; depth 48.83 2024-11-06T04:55:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868943.62 epoch sec; seqNo:3035; 4061709.11 north; 594901.46 east; 36.70 lat; -121.94 lon; 48.83 depth; utmZone: 10 2024-11-06T04:55:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868939.99 2024-11-06T04:55:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:43Z- waiting for 1730868943.99, time is 1730868943.62 2024-11-06T04:55:44Z- AHRS_M2 msg: 1730868944.01 epoch sec; seqNo:2845 2024-11-06T04:55:44Z- AHRS_M2 msg: 0.03 phi; 0.07 theta; 3.89 psi 2024-11-06T04:55:44Z- handleDepth()- Depth_Keller msg: 1730868944.02 epoch sec; seqNo:3036; depth 48.81 2024-11-06T04:55:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868944.03 epoch sec; seqNo:3036; 4061708.73 north; 594901.47 east; 36.70 lat; -121.94 lon; 48.81 depth; utmZone: 10 2024-11-06T04:55:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868939.99 2024-11-06T04:55:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:44Z- waiting for 1730868943.99, time is 1730868944.03 2024-11-06T04:55:44Z- LcmTrn::updateTrn() >>>> pose time 1730868944.03, meas time 1730868943.21 <<<< 2024-11-06T04:55:44Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868944.03 2024-11-06T04:55:44Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:55:44Z- TerrainNav::motionUpdate() - currEstimate = (4061709, 594901.47, 48.81) 2024-11-06T04:55:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:55:44Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:55:44Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:55:44Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:55:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:55:44Z- variances([0], [2], and psi_berg) = 0.583336 0.221841 0.000000 2024-11-06T04:55:44Z- publishEstimatesDecoder() - at 1730868944.03 sec 2024-11-06T04:55:44Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:55:44Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061653.05 594906.34 55.72 2024-11-06T04:55:44Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061652.20 594906.04 56.83 lat/lon: 36.70 -121.94 2024-11-06T04:55:44Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.58 0.22 1.21 0.00 2024-11-06T04:55:44Z- AHRS_M2 msg: 1730868944.41 epoch sec; seqNo:2846 2024-11-06T04:55:44Z- AHRS_M2 msg: 0.03 phi; 0.06 theta; 3.87 psi 2024-11-06T04:55:44Z- handleDepth()- Depth_Keller msg: 1730868944.42 epoch sec; seqNo:3037; depth 48.78 2024-11-06T04:55:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868944.44 epoch sec; seqNo:3037; 4061708.36 north; 594901.47 east; 36.70 lat; -121.94 lon; 48.78 depth; utmZone: 10 2024-11-06T04:55:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868944.03 2024-11-06T04:55:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:44Z- waiting for 1730868948.03, time is 1730868944.44 2024-11-06T04:55:44Z- AHRS_M2 msg: 1730868944.82 epoch sec; seqNo:2847 2024-11-06T04:55:44Z- AHRS_M2 msg: 0.03 phi; 0.06 theta; 3.86 psi 2024-11-06T04:55:44Z- handleDepth()- Depth_Keller msg: 1730868944.83 epoch sec; seqNo:3038; depth 48.75 2024-11-06T04:55:44Z- handleDVL() - DVL msg epochMillisec: 1730868944.86 2024-11-06T04:55:44Z- handleDvl() - RDI_Pathfinder msg: 1730868944.86 sec; seqNo:1096; #beams:4 2024-11-06T04:55:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 43.73 , 47.26 , 43.73 2024-11-06T04:55:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.08 , 0.03 2024-11-06T04:55:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868944.87 epoch sec; seqNo:3038; 4061708.36 north; 594901.47 east; 36.70 lat; -121.94 lon; 48.75 depth; utmZone: 10 2024-11-06T04:55:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868944.03 2024-11-06T04:55:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:44Z- waiting for 1730868948.03, time is 1730868944.87 2024-11-06T04:55:45Z- AHRS_M2 msg: 1730868945.22 epoch sec; seqNo:2848 2024-11-06T04:55:45Z- AHRS_M2 msg: 0.03 phi; 0.05 theta; 3.85 psi 2024-11-06T04:55:45Z- handleDepth()- Depth_Keller msg: 1730868945.22 epoch sec; seqNo:3039; depth 48.73 2024-11-06T04:55:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868945.24 epoch sec; seqNo:3039; 4061707.98 north; 594901.48 east; 36.70 lat; -121.94 lon; 48.73 depth; utmZone: 10 2024-11-06T04:55:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868944.03 2024-11-06T04:55:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:45Z- waiting for 1730868948.03, time is 1730868945.24 2024-11-06T04:55:45Z- AHRS_M2 msg: 1730868945.62 epoch sec; seqNo:2849 2024-11-06T04:55:45Z- AHRS_M2 msg: 0.03 phi; 0.05 theta; 3.84 psi 2024-11-06T04:55:45Z- handleDepth()- Depth_Keller msg: 1730868945.63 epoch sec; seqNo:3040; depth 48.68 2024-11-06T04:55:45Z- handleDVL() - DVL msg epochMillisec: 1730868945.64 2024-11-06T04:55:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:55:45Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:55:45Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:55:45Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:55:45Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:55:45Z- handleDvl() - RDI_Pathfinder msg: 1730868945.64 sec; seqNo:1097; #beams:0 2024-11-06T04:55:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:55:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.08 , 0.03 2024-11-06T04:55:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868945.65 epoch sec; seqNo:3040; 4061707.58 north; 594900.26 east; 36.70 lat; -121.94 lon; 48.68 depth; utmZone: 10 2024-11-06T04:55:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868944.03 2024-11-06T04:55:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:45Z- waiting for 1730868948.03, time is 1730868945.65 2024-11-06T04:55:46Z- AHRS_M2 msg: 1730868946.03 epoch sec; seqNo:2850 2024-11-06T04:55:46Z- AHRS_M2 msg: 0.03 phi; 0.04 theta; 3.82 psi 2024-11-06T04:55:46Z- handleDepth()- Depth_Keller msg: 1730868946.04 epoch sec; seqNo:3041; depth 48.67 2024-11-06T04:55:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868946.05 epoch sec; seqNo:3041; 4061707.58 north; 594900.26 east; 36.70 lat; -121.94 lon; 48.67 depth; utmZone: 10 2024-11-06T04:55:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868944.03 2024-11-06T04:55:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:46Z- waiting for 1730868948.03, time is 1730868946.05 2024-11-06T04:55:46Z- AHRS_M2 msg: 1730868946.43 epoch sec; seqNo:2851 2024-11-06T04:55:46Z- AHRS_M2 msg: 0.03 phi; 0.04 theta; 3.81 psi 2024-11-06T04:55:46Z- handleDepth()- Depth_Keller msg: 1730868946.44 epoch sec; seqNo:3042; depth 48.67 2024-11-06T04:55:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868946.45 epoch sec; seqNo:3042; 4061707.21 north; 594900.27 east; 36.70 lat; -121.94 lon; 48.67 depth; utmZone: 10 2024-11-06T04:55:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868944.03 2024-11-06T04:55:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:46Z- waiting for 1730868948.03, time is 1730868946.45 2024-11-06T04:55:46Z- AHRS_M2 msg: 1730868946.84 epoch sec; seqNo:2852 2024-11-06T04:55:46Z- AHRS_M2 msg: 0.03 phi; 0.03 theta; 3.80 psi 2024-11-06T04:55:46Z- handleDepth()- Depth_Keller msg: 1730868946.84 epoch sec; seqNo:3043; depth 48.62 2024-11-06T04:55:46Z- handleDVL() - DVL msg epochMillisec: 1730868946.86 2024-11-06T04:55:46Z- handleDvl() - RDI_Pathfinder msg: 1730868946.86 sec; seqNo:1098; #beams:4 2024-11-06T04:55:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.92 , 43.07 , 47.04 , 44.39 2024-11-06T04:55:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , 0.07 2024-11-06T04:55:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868946.89 epoch sec; seqNo:3043; 4061706.83 north; 594900.27 east; 36.70 lat; -121.94 lon; 48.62 depth; utmZone: 10 2024-11-06T04:55:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868944.03 2024-11-06T04:55:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:46Z- waiting for 1730868948.03, time is 1730868946.89 2024-11-06T04:55:47Z- AHRS_M2 msg: 1730868947.24 epoch sec; seqNo:2853 2024-11-06T04:55:47Z- AHRS_M2 msg: 0.03 phi; 0.03 theta; 3.79 psi 2024-11-06T04:55:47Z- handleDepth()- Depth_Keller msg: 1730868947.26 epoch sec; seqNo:3044; depth 48.60 2024-11-06T04:55:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868947.27 epoch sec; seqNo:3044; 4061706.45 north; 594900.27 east; 36.70 lat; -121.94 lon; 48.60 depth; utmZone: 10 2024-11-06T04:55:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868944.03 2024-11-06T04:55:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:47Z- waiting for 1730868948.03, time is 1730868947.27 2024-11-06T04:55:47Z- AHRS_M2 msg: 1730868947.64 epoch sec; seqNo:2854 2024-11-06T04:55:47Z- AHRS_M2 msg: 0.03 phi; 0.02 theta; 3.78 psi 2024-11-06T04:55:47Z- handleDepth()- Depth_Keller msg: 1730868947.65 epoch sec; seqNo:3045; depth 48.56 2024-11-06T04:55:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868947.67 epoch sec; seqNo:3045; 4061706.43 north; 594899.05 east; 36.70 lat; -121.94 lon; 48.56 depth; utmZone: 10 2024-11-06T04:55:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868944.03 2024-11-06T04:55:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:47Z- waiting for 1730868948.03, time is 1730868947.67 2024-11-06T04:55:48Z- AHRS_M2 msg: 1730868948.05 epoch sec; seqNo:2855 2024-11-06T04:55:48Z- AHRS_M2 msg: 0.03 phi; 0.02 theta; 3.77 psi 2024-11-06T04:55:48Z- handleDepth()- Depth_Keller msg: 1730868948.05 epoch sec; seqNo:3046; depth 48.56 2024-11-06T04:55:48Z- handleDVL() - DVL msg epochMillisec: 1730868948.06 2024-11-06T04:55:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:55:48Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:55:48Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:55:48Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:55:48Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:55:48Z- handleDvl() - RDI_Pathfinder msg: 1730868948.06 sec; seqNo:1099; #beams:0 2024-11-06T04:55:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:55:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , 0.07 2024-11-06T04:55:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868948.07 epoch sec; seqNo:3046; 4061706.06 north; 594899.06 east; 36.70 lat; -121.94 lon; 48.56 depth; utmZone: 10 2024-11-06T04:55:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868944.03 2024-11-06T04:55:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:48Z- waiting for 1730868948.03, time is 1730868948.07 2024-11-06T04:55:48Z- LcmTrn::updateTrn() >>>> pose time 1730868948.07, meas time 1730868948.06 <<<< 2024-11-06T04:55:48Z- LcmTrn::updateTrn() equating pose 1730868948.06 and meas times 1730868948.06 2024-11-06T04:55:48Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868948.06 2024-11-06T04:55:48Z- TerrainNav::motionUpdate() - currEstimate = (4061706, 594899.06, 48.56) 2024-11-06T04:55:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:55:48Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:55:48Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:55:48Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:55:48Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:55:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:55:48Z- variances([0], [2], and psi_berg) = 0.585761 0.223069 0.000000 2024-11-06T04:55:48Z- publishEstimatesDecoder() - at 1730868948.06 sec 2024-11-06T04:55:48Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:55:48Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061650.45 594903.93 55.57 2024-11-06T04:55:48Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061649.52 594903.63 56.56 lat/lon: 36.70 -121.94 2024-11-06T04:55:48Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.59 0.22 1.46 0.00 2024-11-06T04:55:48Z- AHRS_M2 msg: 1730868948.45 epoch sec; seqNo:2856 2024-11-06T04:55:48Z- AHRS_M2 msg: 0.03 phi; 0.02 theta; 3.76 psi 2024-11-06T04:55:48Z- handleDepth()- Depth_Keller msg: 1730868948.45 epoch sec; seqNo:3047; depth 48.53 2024-11-06T04:55:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868948.48 epoch sec; seqNo:3047; 4061705.68 north; 594899.06 east; 36.70 lat; -121.94 lon; 48.53 depth; utmZone: 10 2024-11-06T04:55:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868948.06 2024-11-06T04:55:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:48Z- waiting for 1730868952.06, time is 1730868948.48 2024-11-06T04:55:48Z- AHRS_M2 msg: 1730868948.86 epoch sec; seqNo:2857 2024-11-06T04:55:48Z- AHRS_M2 msg: 0.04 phi; 0.01 theta; 3.75 psi 2024-11-06T04:55:48Z- handleDepth()- Depth_Keller msg: 1730868948.86 epoch sec; seqNo:3048; depth 48.50 2024-11-06T04:55:48Z- handleDVL() - DVL msg epochMillisec: 1730868948.88 2024-11-06T04:55:48Z- handleDvl() - RDI_Pathfinder msg: 1730868948.88 sec; seqNo:1100; #beams:4 2024-11-06T04:55:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.92 , 42.85 , 46.82 , 45.28 2024-11-06T04:55:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , 0.05 2024-11-06T04:55:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868948.90 epoch sec; seqNo:3048; 4061705.30 north; 594899.07 east; 36.70 lat; -121.94 lon; 48.50 depth; utmZone: 10 2024-11-06T04:55:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868948.06 2024-11-06T04:55:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:49Z- waiting for 1730868952.06, time is 1730868948.90 2024-11-06T04:55:49Z- AHRS_M2 msg: 1730868949.26 epoch sec; seqNo:2858 2024-11-06T04:55:49Z- AHRS_M2 msg: 0.04 phi; 0.01 theta; 3.75 psi 2024-11-06T04:55:49Z- handleDepth()- Depth_Keller msg: 1730868949.27 epoch sec; seqNo:3049; depth 48.50 2024-11-06T04:55:49Z- handleDVL() - DVL msg epochMillisec: 1730868949.28 2024-11-06T04:55:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:55:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:55:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:55:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:55:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:55:49Z- handleDvl() - RDI_Pathfinder msg: 1730868949.28 sec; seqNo:1101; #beams:0 2024-11-06T04:55:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:55:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , 0.05 2024-11-06T04:55:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868949.30 epoch sec; seqNo:3049; 4061704.92 north; 594899.07 east; 36.70 lat; -121.94 lon; 48.50 depth; utmZone: 10 2024-11-06T04:55:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868948.06 2024-11-06T04:55:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:49Z- waiting for 1730868952.06, time is 1730868949.30 2024-11-06T04:55:49Z- AHRS_M2 msg: 1730868949.67 epoch sec; seqNo:2859 2024-11-06T04:55:49Z- AHRS_M2 msg: 0.05 phi; 0.00 theta; 3.74 psi 2024-11-06T04:55:49Z- handleDepth()- Depth_Keller msg: 1730868949.67 epoch sec; seqNo:3050; depth 48.48 2024-11-06T04:55:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868949.68 epoch sec; seqNo:3050; 4061704.92 north; 594899.07 east; 36.70 lat; -121.94 lon; 48.48 depth; utmZone: 10 2024-11-06T04:55:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868948.06 2024-11-06T04:55:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:49Z- waiting for 1730868952.06, time is 1730868949.68 2024-11-06T04:55:50Z- AHRS_M2 msg: 1730868950.07 epoch sec; seqNo:2860 2024-11-06T04:55:50Z- AHRS_M2 msg: 0.05 phi; -0.00 theta; 3.73 psi 2024-11-06T04:55:50Z- handleDepth()- Depth_Keller msg: 1730868950.08 epoch sec; seqNo:3051; depth 48.48 2024-11-06T04:55:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868950.09 epoch sec; seqNo:3051; 4061704.53 north; 594897.85 east; 36.70 lat; -121.94 lon; 48.48 depth; utmZone: 10 2024-11-06T04:55:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868948.06 2024-11-06T04:55:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:50Z- waiting for 1730868952.06, time is 1730868950.09 2024-11-06T04:55:50Z- AHRS_M2 msg: 1730868950.47 epoch sec; seqNo:2861 2024-11-06T04:55:50Z- AHRS_M2 msg: 0.06 phi; -0.01 theta; 3.73 psi 2024-11-06T04:55:50Z- handleDepth()- Depth_Keller msg: 1730868950.48 epoch sec; seqNo:3052; depth 48.45 2024-11-06T04:55:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868950.50 epoch sec; seqNo:3052; 4061704.15 north; 594897.86 east; 36.70 lat; -121.94 lon; 48.45 depth; utmZone: 10 2024-11-06T04:55:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868948.06 2024-11-06T04:55:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:50Z- waiting for 1730868952.06, time is 1730868950.50 2024-11-06T04:55:50Z- AHRS_M2 msg: 1730868950.88 epoch sec; seqNo:2862 2024-11-06T04:55:50Z- AHRS_M2 msg: 0.06 phi; -0.01 theta; 3.72 psi 2024-11-06T04:55:50Z- handleDepth()- Depth_Keller msg: 1730868950.88 epoch sec; seqNo:3053; depth 48.43 2024-11-06T04:55:50Z- handleDVL() - DVL msg epochMillisec: 1730868950.90 2024-11-06T04:55:50Z- handleDvl() - RDI_Pathfinder msg: 1730868950.90 sec; seqNo:1102; #beams:4 2024-11-06T04:55:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.81 , 42.85 , 46.38 , 45.94 2024-11-06T04:55:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.07 , -0.02 2024-11-06T04:55:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868950.92 epoch sec; seqNo:3053; 4061703.77 north; 594897.86 east; 36.70 lat; -121.94 lon; 48.43 depth; utmZone: 10 2024-11-06T04:55:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868948.06 2024-11-06T04:55:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:51Z- waiting for 1730868952.06, time is 1730868950.92 2024-11-06T04:55:51Z- AHRS_M2 msg: 1730868951.29 epoch sec; seqNo:2863 2024-11-06T04:55:51Z- AHRS_M2 msg: 0.06 phi; -0.01 theta; 3.71 psi 2024-11-06T04:55:51Z- handleDepth()- Depth_Keller msg: 1730868951.30 epoch sec; seqNo:3054; depth 48.42 2024-11-06T04:55:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868951.32 epoch sec; seqNo:3054; 4061703.39 north; 594897.87 east; 36.70 lat; -121.94 lon; 48.42 depth; utmZone: 10 2024-11-06T04:55:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868948.06 2024-11-06T04:55:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:51Z- waiting for 1730868952.06, time is 1730868951.32 2024-11-06T04:55:51Z- AHRS_M2 msg: 1730868951.68 epoch sec; seqNo:2864 2024-11-06T04:55:51Z- AHRS_M2 msg: 0.06 phi; -0.02 theta; 3.71 psi 2024-11-06T04:55:51Z- handleDepth()- Depth_Keller msg: 1730868951.69 epoch sec; seqNo:3055; depth 48.40 2024-11-06T04:55:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868951.71 epoch sec; seqNo:3055; 4061703.01 north; 594897.87 east; 36.70 lat; -121.94 lon; 48.40 depth; utmZone: 10 2024-11-06T04:55:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868948.06 2024-11-06T04:55:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:51Z- waiting for 1730868952.06, time is 1730868951.71 2024-11-06T04:55:52Z- AHRS_M2 msg: 1730868952.09 epoch sec; seqNo:2865 2024-11-06T04:55:52Z- AHRS_M2 msg: 0.06 phi; -0.02 theta; 3.70 psi 2024-11-06T04:55:52Z- handleDepth()- Depth_Keller msg: 1730868952.09 epoch sec; seqNo:3056; depth 48.39 2024-11-06T04:55:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868952.11 epoch sec; seqNo:3056; 4061702.63 north; 594897.88 east; 36.70 lat; -121.94 lon; 48.39 depth; utmZone: 10 2024-11-06T04:55:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868948.06 2024-11-06T04:55:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:52Z- waiting for 1730868952.06, time is 1730868952.11 2024-11-06T04:55:52Z- LcmTrn::updateTrn() >>>> pose time 1730868952.11, meas time 1730868950.90 <<<< 2024-11-06T04:55:52Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868952.11 2024-11-06T04:55:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868950.90 sec, ping # 0; waiting for INS data more recent than 1730868948.06... 2024-11-06T04:55:52Z- TerrainNav::motionUpdate() - currEstimate = (4061703, 594897.88, 48.39) 2024-11-06T04:55:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:55:52Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:55:52Z- TNavPF::Projecting Measurements 2024-11-06T04:55:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:55:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:55:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:55:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:55:52Z- TNavFilter::4 good beams. 2024-11-06T04:55:52Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:55:52Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:55:52Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:55:52Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:55:52Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:55:52Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:55:52Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:55:52Z- TerrainMapOctree: (vehN, vehE ) = (4061703.65, 594898.23). 2024-11-06T04:55:52Z- TerrainMapOctree: Min Distance = 155.30. 2024-11-06T04:55:52Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:55:52Z- Forcing Subcloud Comparison 2024-11-06T04:55:52Z- Weighting particles with subcloud comparison 2024-11-06T04:55:52Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:52Z- meanExpectedMeasDiff: 0.919351 delta_rms_squared: 2.395414 alpha: 0.991720 2024-11-06T04:55:52Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:52Z- meanExpectedMeasDiff: -2.383610 delta_rms_squared: 2.135617 alpha: 0.990722 2024-11-06T04:55:52Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:52Z- meanExpectedMeasDiff: 0.091169 delta_rms_squared: 2.237289 alpha: 0.991140 2024-11-06T04:55:52Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:55:52Z- meanExpectedMeasDiff: -0.818793 delta_rms_squared: 2.282957 alpha: 0.991316 2024-11-06T04:55:52Z- Subcloud NIS: 0.357794 2024-11-06T04:55:52Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:55:52Z- TNavPF:: sumWeights = 0.312025 2024-11-06T04:55:52Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:55:52Z- TNavPF::First Term of Map Covariance Matrix is 0.53 2024-11-06T04:55:52Z- TNavPF::Calculated NIS Value : 0.15 numBeams normalized NIS: 0.04 2024-11-06T04:55:52Z- TerrainNav::Resampling particle filter... 2024-11-06T04:55:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868950.90 sec. 2024-11-06T04:55:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:55:52Z- variances([0], [2], and psi_berg) = 0.592711 0.227098 0.000000 2024-11-06T04:55:52Z- publishEstimatesDecoder() - at 1730868952.11 sec 2024-11-06T04:55:52Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:55:52Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061647.78 594902.26 57.13 2024-11-06T04:55:52Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061646.10 594902.45 56.72 lat/lon: 36.70 -121.94 2024-11-06T04:55:52Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.59 0.23 0.87 0.00 2024-11-06T04:55:52Z- AHRS_M2 msg: 1730868952.49 epoch sec; seqNo:2866 2024-11-06T04:55:52Z- AHRS_M2 msg: 0.06 phi; -0.02 theta; 3.69 psi 2024-11-06T04:55:52Z- handleDepth()- Depth_Keller msg: 1730868952.50 epoch sec; seqNo:3057; depth 48.37 2024-11-06T04:55:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868952.52 epoch sec; seqNo:3057; 4061702.25 north; 594897.88 east; 36.70 lat; -121.94 lon; 48.37 depth; utmZone: 10 2024-11-06T04:55:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868952.11 2024-11-06T04:55:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:52Z- waiting for 1730868956.11, time is 1730868952.52 2024-11-06T04:55:52Z- AHRS_M2 msg: 1730868952.90 epoch sec; seqNo:2867 2024-11-06T04:55:52Z- AHRS_M2 msg: 0.06 phi; -0.03 theta; 3.68 psi 2024-11-06T04:55:52Z- handleDepth()- Depth_Keller msg: 1730868952.91 epoch sec; seqNo:3058; depth 48.36 2024-11-06T04:55:52Z- handleDVL() - DVL msg epochMillisec: 1730868952.93 2024-11-06T04:55:52Z- handleDvl() - RDI_Pathfinder msg: 1730868952.93 sec; seqNo:1103; #beams:4 2024-11-06T04:55:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.03 , 42.40 , 45.72 , 46.38 2024-11-06T04:55:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , -0.06 2024-11-06T04:55:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868952.94 epoch sec; seqNo:3058; 4061702.24 north; 594896.66 east; 36.70 lat; -121.94 lon; 48.36 depth; utmZone: 10 2024-11-06T04:55:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868952.11 2024-11-06T04:55:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:53Z- waiting for 1730868956.11, time is 1730868952.94 2024-11-06T04:55:53Z- AHRS_M2 msg: 1730868953.30 epoch sec; seqNo:2868 2024-11-06T04:55:53Z- AHRS_M2 msg: 0.06 phi; -0.03 theta; 3.68 psi 2024-11-06T04:55:53Z- handleDepth()- Depth_Keller msg: 1730868953.31 epoch sec; seqNo:3059; depth 48.36 2024-11-06T04:55:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868953.33 epoch sec; seqNo:3059; 4061701.86 north; 594896.66 east; 36.70 lat; -121.94 lon; 48.36 depth; utmZone: 10 2024-11-06T04:55:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868952.11 2024-11-06T04:55:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:53Z- waiting for 1730868956.11, time is 1730868953.33 2024-11-06T04:55:53Z- AHRS_M2 msg: 1730868953.70 epoch sec; seqNo:2869 2024-11-06T04:55:53Z- AHRS_M2 msg: 0.07 phi; -0.03 theta; 3.67 psi 2024-11-06T04:55:53Z- handleDepth()- Depth_Keller msg: 1730868953.71 epoch sec; seqNo:3060; depth 48.36 2024-11-06T04:55:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868953.72 epoch sec; seqNo:3060; 4061701.48 north; 594896.67 east; 36.70 lat; -121.94 lon; 48.36 depth; utmZone: 10 2024-11-06T04:55:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868952.11 2024-11-06T04:55:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:53Z- waiting for 1730868956.11, time is 1730868953.72 2024-11-06T04:55:54Z- AHRS_M2 msg: 1730868954.11 epoch sec; seqNo:2870 2024-11-06T04:55:54Z- AHRS_M2 msg: 0.07 phi; -0.04 theta; 3.66 psi 2024-11-06T04:55:54Z- handleDepth()- Depth_Keller msg: 1730868954.12 epoch sec; seqNo:3061; depth 48.34 2024-11-06T04:55:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868954.12 epoch sec; seqNo:3061; 4061701.10 north; 594896.67 east; 36.70 lat; -121.94 lon; 48.34 depth; utmZone: 10 2024-11-06T04:55:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868952.11 2024-11-06T04:55:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:54Z- waiting for 1730868956.11, time is 1730868954.12 2024-11-06T04:55:54Z- AHRS_M2 msg: 1730868954.51 epoch sec; seqNo:2871 2024-11-06T04:55:54Z- AHRS_M2 msg: 0.07 phi; -0.04 theta; 3.65 psi 2024-11-06T04:55:54Z- handleDepth()- Depth_Keller msg: 1730868954.52 epoch sec; seqNo:3062; depth 48.34 2024-11-06T04:55:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868954.53 epoch sec; seqNo:3062; 4061700.72 north; 594896.68 east; 36.70 lat; -121.94 lon; 48.34 depth; utmZone: 10 2024-11-06T04:55:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868952.11 2024-11-06T04:55:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:54Z- waiting for 1730868956.11, time is 1730868954.53 2024-11-06T04:55:54Z- AHRS_M2 msg: 1730868954.91 epoch sec; seqNo:2872 2024-11-06T04:55:54Z- AHRS_M2 msg: 0.07 phi; -0.04 theta; 3.64 psi 2024-11-06T04:55:54Z- handleDepth()- Depth_Keller msg: 1730868954.92 epoch sec; seqNo:3063; depth 48.34 2024-11-06T04:55:54Z- handleDVL() - DVL msg epochMillisec: 1730868954.95 2024-11-06T04:55:54Z- handleDvl() - RDI_Pathfinder msg: 1730868954.95 sec; seqNo:1104; #beams:4 2024-11-06T04:55:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.03 , 41.74 , 45.50 , 46.60 2024-11-06T04:55:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.11 , -0.03 2024-11-06T04:55:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868954.96 epoch sec; seqNo:3063; 4061700.35 north; 594896.68 east; 36.70 lat; -121.94 lon; 48.34 depth; utmZone: 10 2024-11-06T04:55:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868952.11 2024-11-06T04:55:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:55Z- waiting for 1730868956.11, time is 1730868954.96 2024-11-06T04:55:55Z- AHRS_M2 msg: 1730868955.33 epoch sec; seqNo:2873 2024-11-06T04:55:55Z- AHRS_M2 msg: 0.07 phi; -0.05 theta; 3.62 psi 2024-11-06T04:55:55Z- handleDepth()- Depth_Keller msg: 1730868955.34 epoch sec; seqNo:3064; depth 48.33 2024-11-06T04:55:55Z- handleDVL() - DVL msg epochMillisec: 1730868955.34 2024-11-06T04:55:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:55:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:55:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:55:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:55:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:55:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:55:55Z- handleDvl() - RDI_Pathfinder msg: 1730868955.34 sec; seqNo:1105; #beams:0 2024-11-06T04:55:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:55:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.11 , -0.03 2024-11-06T04:55:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868955.36 epoch sec; seqNo:3064; 4061700.33 north; 594895.46 east; 36.70 lat; -121.94 lon; 48.33 depth; utmZone: 10 2024-11-06T04:55:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868952.11 2024-11-06T04:55:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:55Z- waiting for 1730868956.11, time is 1730868955.36 2024-11-06T04:55:55Z- AHRS_M2 msg: 1730868955.73 epoch sec; seqNo:2874 2024-11-06T04:55:55Z- AHRS_M2 msg: 0.07 phi; -0.05 theta; 3.61 psi 2024-11-06T04:55:55Z- handleDepth()- Depth_Keller msg: 1730868955.73 epoch sec; seqNo:3065; depth 48.33 2024-11-06T04:55:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868955.75 epoch sec; seqNo:3065; 4061699.95 north; 594895.46 east; 36.70 lat; -121.94 lon; 48.33 depth; utmZone: 10 2024-11-06T04:55:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868952.11 2024-11-06T04:55:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:55Z- waiting for 1730868956.11, time is 1730868955.75 2024-11-06T04:55:56Z- handleDepth()- Depth_Keller msg: 1730868956.16 epoch sec; seqNo:3066; depth 48.34 2024-11-06T04:55:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868956.17 epoch sec; seqNo:3066; 4061699.57 north; 594895.47 east; 36.70 lat; -121.94 lon; 48.34 depth; utmZone: 10 2024-11-06T04:55:56Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:55:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868952.11 2024-11-06T04:55:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:56Z- waiting for 1730868956.11, time is 1730868956.17 2024-11-06T04:55:56Z- LcmTrn::updateTrn() >>>> pose time 1730868956.17, meas time 1730868955.34 <<<< 2024-11-06T04:55:56Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868956.17 2024-11-06T04:55:56Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:55:56Z- TerrainNav::motionUpdate() - currEstimate = (4061700, 594895.47, 48.34) 2024-11-06T04:55:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:55:56Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:55:56Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:55:56Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:55:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:55:56Z- variances([0], [2], and psi_berg) = 0.594400 0.229202 0.000000 2024-11-06T04:55:56Z- publishEstimatesDecoder() - at 1730868956.17 sec 2024-11-06T04:55:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:55:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061644.77 594899.87 57.89 2024-11-06T04:55:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061643.04 594900.04 56.68 lat/lon: 36.70 -121.94 2024-11-06T04:55:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.59 0.23 1.11 0.00 2024-11-06T04:55:56Z- AHRS_M2 msg: 1730868956.53 epoch sec; seqNo:2875 2024-11-06T04:55:56Z- AHRS_M2 msg: 0.07 phi; -0.06 theta; 3.60 psi 2024-11-06T04:55:56Z- handleDepth()- Depth_Keller msg: 1730868956.54 epoch sec; seqNo:3067; depth 48.35 2024-11-06T04:55:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868956.55 epoch sec; seqNo:3067; 4061699.20 north; 594895.47 east; 36.70 lat; -121.94 lon; 48.35 depth; utmZone: 10 2024-11-06T04:55:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868956.17 2024-11-06T04:55:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:56Z- waiting for 1730868960.17, time is 1730868956.55 2024-11-06T04:55:56Z- AHRS_M2 msg: 1730868956.93 epoch sec; seqNo:2876 2024-11-06T04:55:56Z- AHRS_M2 msg: 0.08 phi; -0.06 theta; 3.59 psi 2024-11-06T04:55:56Z- handleDepth()- Depth_Keller msg: 1730868956.94 epoch sec; seqNo:3068; depth 48.37 2024-11-06T04:55:56Z- handleDVL() - DVL msg epochMillisec: 1730868956.97 2024-11-06T04:55:56Z- handleDvl() - RDI_Pathfinder msg: 1730868956.97 sec; seqNo:1106; #beams:4 2024-11-06T04:55:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.25 , 41.52 , 45.05 , 47.26 2024-11-06T04:55:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.15 , 0.05 2024-11-06T04:55:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868956.98 epoch sec; seqNo:3068; 4061698.82 north; 594895.48 east; 36.70 lat; -121.94 lon; 48.37 depth; utmZone: 10 2024-11-06T04:55:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868956.17 2024-11-06T04:55:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:57Z- waiting for 1730868960.17, time is 1730868956.98 2024-11-06T04:55:57Z- AHRS_M2 msg: 1730868957.34 epoch sec; seqNo:2877 2024-11-06T04:55:57Z- AHRS_M2 msg: 0.08 phi; -0.06 theta; 3.58 psi 2024-11-06T04:55:57Z- handleDepth()- Depth_Keller msg: 1730868957.35 epoch sec; seqNo:3069; depth 48.37 2024-11-06T04:55:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868957.37 epoch sec; seqNo:3069; 4061698.82 north; 594895.48 east; 36.70 lat; -121.94 lon; 48.37 depth; utmZone: 10 2024-11-06T04:55:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868956.17 2024-11-06T04:55:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:57Z- waiting for 1730868960.17, time is 1730868957.37 2024-11-06T04:55:57Z- AHRS_M2 msg: 1730868957.74 epoch sec; seqNo:2878 2024-11-06T04:55:57Z- AHRS_M2 msg: 0.08 phi; -0.07 theta; 3.57 psi 2024-11-06T04:55:57Z- handleDepth()- Depth_Keller msg: 1730868957.75 epoch sec; seqNo:3070; depth 48.38 2024-11-06T04:55:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868957.76 epoch sec; seqNo:3070; 4061698.42 north; 594894.26 east; 36.70 lat; -121.94 lon; 48.38 depth; utmZone: 10 2024-11-06T04:55:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868956.17 2024-11-06T04:55:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:57Z- waiting for 1730868960.17, time is 1730868957.76 2024-11-06T04:55:58Z- AHRS_M2 msg: 1730868958.15 epoch sec; seqNo:2879 2024-11-06T04:55:58Z- AHRS_M2 msg: 0.09 phi; -0.07 theta; 3.57 psi 2024-11-06T04:55:58Z- handleDepth()- Depth_Keller msg: 1730868958.17 epoch sec; seqNo:3071; depth 48.41 2024-11-06T04:55:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868958.18 epoch sec; seqNo:3071; 4061698.05 north; 594894.26 east; 36.70 lat; -121.94 lon; 48.41 depth; utmZone: 10 2024-11-06T04:55:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868956.17 2024-11-06T04:55:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:58Z- waiting for 1730868960.17, time is 1730868958.18 2024-11-06T04:55:58Z- AHRS_M2 msg: 1730868958.55 epoch sec; seqNo:2880 2024-11-06T04:55:58Z- AHRS_M2 msg: 0.09 phi; -0.07 theta; 3.56 psi 2024-11-06T04:55:58Z- handleDepth()- Depth_Keller msg: 1730868958.56 epoch sec; seqNo:3072; depth 48.43 2024-11-06T04:55:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868958.57 epoch sec; seqNo:3072; 4061697.67 north; 594894.27 east; 36.70 lat; -121.94 lon; 48.43 depth; utmZone: 10 2024-11-06T04:55:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868956.17 2024-11-06T04:55:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:58Z- waiting for 1730868960.17, time is 1730868958.57 2024-11-06T04:55:58Z- AHRS_M2 msg: 1730868958.96 epoch sec; seqNo:2881 2024-11-06T04:55:58Z- AHRS_M2 msg: 0.09 phi; -0.07 theta; 3.56 psi 2024-11-06T04:55:58Z- handleDepth()- Depth_Keller msg: 1730868958.97 epoch sec; seqNo:3073; depth 48.44 2024-11-06T04:55:58Z- handleDVL() - DVL msg epochMillisec: 1730868958.99 2024-11-06T04:55:58Z- handleDvl() - RDI_Pathfinder msg: 1730868958.99 sec; seqNo:1107; #beams:4 2024-11-06T04:55:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.69 , 41.30 , 44.83 , 47.48 2024-11-06T04:55:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.08 , 0.09 2024-11-06T04:55:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868959.00 epoch sec; seqNo:3073; 4061697.29 north; 594894.27 east; 36.70 lat; -121.94 lon; 48.44 depth; utmZone: 10 2024-11-06T04:55:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:55:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868956.17 2024-11-06T04:55:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:59Z- waiting for 1730868960.17, time is 1730868959.00 2024-11-06T04:55:59Z- AHRS_M2 msg: 1730868959.36 epoch sec; seqNo:2882 2024-11-06T04:55:59Z- AHRS_M2 msg: 0.09 phi; -0.07 theta; 3.55 psi 2024-11-06T04:55:59Z- handleDepth()- Depth_Keller msg: 1730868959.38 epoch sec; seqNo:3074; depth 48.46 2024-11-06T04:55:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868959.39 epoch sec; seqNo:3074; 4061696.91 north; 594894.28 east; 36.70 lat; -121.94 lon; 48.46 depth; utmZone: 10 2024-11-06T04:55:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868956.17 2024-11-06T04:55:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:59Z- waiting for 1730868960.17, time is 1730868959.39 2024-11-06T04:55:59Z- AHRS_M2 msg: 1730868959.76 epoch sec; seqNo:2883 2024-11-06T04:55:59Z- AHRS_M2 msg: 0.09 phi; -0.07 theta; 3.55 psi 2024-11-06T04:55:59Z- handleDepth()- Depth_Keller msg: 1730868959.77 epoch sec; seqNo:3075; depth 48.49 2024-11-06T04:55:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868959.78 epoch sec; seqNo:3075; 4061696.91 north; 594894.28 east; 36.70 lat; -121.94 lon; 48.49 depth; utmZone: 10 2024-11-06T04:55:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:55:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868956.17 2024-11-06T04:55:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:55:59Z- waiting for 1730868960.17, time is 1730868959.78 2024-11-06T04:56:00Z- AHRS_M2 msg: 1730868960.17 epoch sec; seqNo:2884 2024-11-06T04:56:00Z- AHRS_M2 msg: 0.09 phi; -0.08 theta; 3.55 psi 2024-11-06T04:56:00Z- handleDepth()- Depth_Keller msg: 1730868960.17 epoch sec; seqNo:3076; depth 48.50 2024-11-06T04:56:00Z- handleDVL() - DVL msg epochMillisec: 1730868960.18 2024-11-06T04:56:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:00Z- handleDvl() - RDI_Pathfinder msg: 1730868960.18 sec; seqNo:1108; #beams:0 2024-11-06T04:56:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.08 , 0.09 2024-11-06T04:56:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868960.19 epoch sec; seqNo:3076; 4061696.52 north; 594893.06 east; 36.70 lat; -121.94 lon; 48.50 depth; utmZone: 10 2024-11-06T04:56:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868956.17 2024-11-06T04:56:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:00Z- waiting for 1730868960.17, time is 1730868960.19 2024-11-06T04:56:00Z- LcmTrn::updateTrn() >>>> pose time 1730868960.19, meas time 1730868960.18 <<<< 2024-11-06T04:56:00Z- LcmTrn::updateTrn() equating pose 1730868960.18 and meas times 1730868960.18 2024-11-06T04:56:00Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730868960.18 2024-11-06T04:56:00Z- TerrainNav::motionUpdate() - currEstimate = (4061697, 594893.06, 48.50) 2024-11-06T04:56:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:56:00Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:56:00Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:56:00Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:56:00Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:56:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:56:00Z- variances([0], [2], and psi_berg) = 0.595952 0.231499 0.000000 2024-11-06T04:56:00Z- publishEstimatesDecoder() - at 1730868960.18 sec 2024-11-06T04:56:00Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:56:00Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061641.69 594897.48 58.55 2024-11-06T04:56:00Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061639.98 594897.63 56.84 lat/lon: 36.70 -121.94 2024-11-06T04:56:00Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.60 0.23 1.36 0.00 2024-11-06T04:56:00Z- AHRS_M2 msg: 1730868960.57 epoch sec; seqNo:2885 2024-11-06T04:56:00Z- AHRS_M2 msg: 0.10 phi; -0.08 theta; 3.55 psi 2024-11-06T04:56:00Z- handleDepth()- Depth_Keller msg: 1730868960.58 epoch sec; seqNo:3077; depth 48.53 2024-11-06T04:56:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868960.59 epoch sec; seqNo:3077; 4061696.14 north; 594893.07 east; 36.70 lat; -121.94 lon; 48.53 depth; utmZone: 10 2024-11-06T04:56:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868960.18 2024-11-06T04:56:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:00Z- waiting for 1730868964.18, time is 1730868960.59 2024-11-06T04:56:00Z- AHRS_M2 msg: 1730868960.98 epoch sec; seqNo:2886 2024-11-06T04:56:00Z- AHRS_M2 msg: 0.10 phi; -0.08 theta; 3.54 psi 2024-11-06T04:56:00Z- handleDepth()- Depth_Keller msg: 1730868960.99 epoch sec; seqNo:3078; depth 48.55 2024-11-06T04:56:01Z- handleDVL() - DVL msg epochMillisec: 1730868961.02 2024-11-06T04:56:01Z- handleDvl() - RDI_Pathfinder msg: 1730868961.02 sec; seqNo:1109; #beams:4 2024-11-06T04:56:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.91 , 41.08 , 44.61 , 47.71 2024-11-06T04:56:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , 0.09 2024-11-06T04:56:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868961.03 epoch sec; seqNo:3078; 4061695.76 north; 594893.07 east; 36.70 lat; -121.94 lon; 48.55 depth; utmZone: 10 2024-11-06T04:56:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868960.18 2024-11-06T04:56:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:01Z- waiting for 1730868964.18, time is 1730868961.03 2024-11-06T04:56:01Z- AHRS_M2 msg: 1730868961.38 epoch sec; seqNo:2887 2024-11-06T04:56:01Z- AHRS_M2 msg: 0.10 phi; -0.08 theta; 3.54 psi 2024-11-06T04:56:01Z- handleDepth()- Depth_Keller msg: 1730868961.40 epoch sec; seqNo:3079; depth 48.57 2024-11-06T04:56:01Z- handleDVL() - DVL msg epochMillisec: 1730868961.40 2024-11-06T04:56:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:01Z- handleDvl() - RDI_Pathfinder msg: 1730868961.40 sec; seqNo:1110; #beams:0 2024-11-06T04:56:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.01 , 0.09 2024-11-06T04:56:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868961.42 epoch sec; seqNo:3079; 4061695.38 north; 594893.07 east; 36.70 lat; -121.94 lon; 48.57 depth; utmZone: 10 2024-11-06T04:56:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868960.18 2024-11-06T04:56:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:01Z- waiting for 1730868964.18, time is 1730868961.42 2024-11-06T04:56:01Z- AHRS_M2 msg: 1730868961.78 epoch sec; seqNo:2888 2024-11-06T04:56:01Z- AHRS_M2 msg: 0.10 phi; -0.08 theta; 3.54 psi 2024-11-06T04:56:01Z- handleDepth()- Depth_Keller msg: 1730868961.79 epoch sec; seqNo:3080; depth 48.61 2024-11-06T04:56:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868961.82 epoch sec; seqNo:3080; 4061695.00 north; 594893.08 east; 36.70 lat; -121.94 lon; 48.61 depth; utmZone: 10 2024-11-06T04:56:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868960.18 2024-11-06T04:56:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:01Z- waiting for 1730868964.18, time is 1730868961.82 2024-11-06T04:56:02Z- AHRS_M2 msg: 1730868962.19 epoch sec; seqNo:2889 2024-11-06T04:56:02Z- AHRS_M2 msg: 0.10 phi; -0.08 theta; 3.53 psi 2024-11-06T04:56:02Z- handleDepth()- Depth_Keller msg: 1730868962.19 epoch sec; seqNo:3081; depth 48.60 2024-11-06T04:56:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868962.20 epoch sec; seqNo:3081; 4061695.00 north; 594893.08 east; 36.70 lat; -121.94 lon; 48.60 depth; utmZone: 10 2024-11-06T04:56:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868960.18 2024-11-06T04:56:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:02Z- waiting for 1730868964.18, time is 1730868962.20 2024-11-06T04:56:02Z- AHRS_M2 msg: 1730868962.59 epoch sec; seqNo:2890 2024-11-06T04:56:02Z- AHRS_M2 msg: 0.10 phi; -0.08 theta; 3.53 psi 2024-11-06T04:56:02Z- handleDepth()- Depth_Keller msg: 1730868962.60 epoch sec; seqNo:3082; depth 48.64 2024-11-06T04:56:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868962.61 epoch sec; seqNo:3082; 4061694.62 north; 594893.08 east; 36.70 lat; -121.94 lon; 48.64 depth; utmZone: 10 2024-11-06T04:56:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868960.18 2024-11-06T04:56:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:02Z- waiting for 1730868964.18, time is 1730868962.61 2024-11-06T04:56:03Z- AHRS_M2 msg: 1730868962.99 epoch sec; seqNo:2891 2024-11-06T04:56:03Z- AHRS_M2 msg: 0.10 phi; -0.08 theta; 3.53 psi 2024-11-06T04:56:03Z- handleDepth()- Depth_Keller msg: 1730868963.00 epoch sec; seqNo:3083; depth 48.65 2024-11-06T04:56:03Z- handleDVL() - DVL msg epochMillisec: 1730868963.03 2024-11-06T04:56:03Z- handleDvl() - RDI_Pathfinder msg: 1730868963.03 sec; seqNo:1111; #beams:4 2024-11-06T04:56:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.35 , 41.08 , 44.39 , 48.15 2024-11-06T04:56:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.07 , 0.04 2024-11-06T04:56:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868963.04 epoch sec; seqNo:3083; 4061694.24 north; 594893.09 east; 36.70 lat; -121.94 lon; 48.65 depth; utmZone: 10 2024-11-06T04:56:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868960.18 2024-11-06T04:56:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:03Z- waiting for 1730868964.18, time is 1730868963.04 2024-11-06T04:56:03Z- AHRS_M2 msg: 1730868963.40 epoch sec; seqNo:2892 2024-11-06T04:56:03Z- AHRS_M2 msg: 0.10 phi; -0.08 theta; 3.52 psi 2024-11-06T04:56:03Z- handleDepth()- Depth_Keller msg: 1730868963.41 epoch sec; seqNo:3084; depth 48.67 2024-11-06T04:56:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868963.43 epoch sec; seqNo:3084; 4061693.86 north; 594893.09 east; 36.70 lat; -121.94 lon; 48.67 depth; utmZone: 10 2024-11-06T04:56:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868960.18 2024-11-06T04:56:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:03Z- waiting for 1730868964.18, time is 1730868963.43 2024-11-06T04:56:03Z- AHRS_M2 msg: 1730868963.81 epoch sec; seqNo:2893 2024-11-06T04:56:03Z- AHRS_M2 msg: 0.10 phi; -0.08 theta; 3.52 psi 2024-11-06T04:56:03Z- handleDepth()- Depth_Keller msg: 1730868963.81 epoch sec; seqNo:3085; depth 48.68 2024-11-06T04:56:03Z- handleDVL() - DVL msg epochMillisec: 1730868963.82 2024-11-06T04:56:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:03Z- handleDvl() - RDI_Pathfinder msg: 1730868963.82 sec; seqNo:1112; #beams:0 2024-11-06T04:56:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.07 , 0.04 2024-11-06T04:56:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868963.83 epoch sec; seqNo:3085; 4061693.49 north; 594893.09 east; 36.70 lat; -121.94 lon; 48.68 depth; utmZone: 10 2024-11-06T04:56:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868960.18 2024-11-06T04:56:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:03Z- waiting for 1730868964.18, time is 1730868963.83 2024-11-06T04:56:04Z- AHRS_M2 msg: 1730868964.21 epoch sec; seqNo:2894 2024-11-06T04:56:04Z- AHRS_M2 msg: 0.10 phi; -0.08 theta; 3.51 psi 2024-11-06T04:56:04Z- handleDepth()- Depth_Keller msg: 1730868964.21 epoch sec; seqNo:3086; depth 48.69 2024-11-06T04:56:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868964.22 epoch sec; seqNo:3086; 4061693.09 north; 594891.88 east; 36.70 lat; -121.94 lon; 48.69 depth; utmZone: 10 2024-11-06T04:56:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868960.18 2024-11-06T04:56:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:04Z- waiting for 1730868964.18, time is 1730868964.22 2024-11-06T04:56:04Z- LcmTrn::updateTrn() >>>> pose time 1730868964.22, meas time 1730868963.82 <<<< 2024-11-06T04:56:04Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868964.22 2024-11-06T04:56:04Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:56:04Z- TerrainNav::motionUpdate() - currEstimate = (4061693, 594891.88, 48.69) 2024-11-06T04:56:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:56:04Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:56:04Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:56:04Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:56:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:56:04Z- variances([0], [2], and psi_berg) = 0.598373 0.233827 0.000000 2024-11-06T04:56:04Z- publishEstimatesDecoder() - at 1730868964.22 sec 2024-11-06T04:56:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:56:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061638.21 594896.24 58.84 2024-11-06T04:56:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061636.56 594896.45 57.02 lat/lon: 36.70 -121.94 2024-11-06T04:56:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.60 0.23 1.60 0.00 2024-11-06T04:56:04Z- AHRS_M2 msg: 1730868964.61 epoch sec; seqNo:2895 2024-11-06T04:56:04Z- AHRS_M2 msg: 0.10 phi; -0.08 theta; 3.51 psi 2024-11-06T04:56:04Z- handleDepth()- Depth_Keller msg: 1730868964.62 epoch sec; seqNo:3087; depth 48.71 2024-11-06T04:56:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868964.63 epoch sec; seqNo:3087; 4061692.71 north; 594891.88 east; 36.70 lat; -121.94 lon; 48.71 depth; utmZone: 10 2024-11-06T04:56:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868964.22 2024-11-06T04:56:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:04Z- waiting for 1730868968.22, time is 1730868964.63 2024-11-06T04:56:05Z- AHRS_M2 msg: 1730868965.02 epoch sec; seqNo:2896 2024-11-06T04:56:05Z- AHRS_M2 msg: 0.09 phi; -0.08 theta; 3.51 psi 2024-11-06T04:56:05Z- handleDepth()- Depth_Keller msg: 1730868965.02 epoch sec; seqNo:3088; depth 48.71 2024-11-06T04:56:05Z- handleDVL() - DVL msg epochMillisec: 1730868965.04 2024-11-06T04:56:05Z- handleDvl() - RDI_Pathfinder msg: 1730868965.04 sec; seqNo:1113; #beams:4 2024-11-06T04:56:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.13 , 40.86 , 44.17 , 48.15 2024-11-06T04:56:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.06 , -0.02 2024-11-06T04:56:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868965.07 epoch sec; seqNo:3088; 4061692.34 north; 594891.89 east; 36.70 lat; -121.94 lon; 48.71 depth; utmZone: 10 2024-11-06T04:56:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868964.22 2024-11-06T04:56:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:05Z- waiting for 1730868968.22, time is 1730868965.07 2024-11-06T04:56:05Z- AHRS_M2 msg: 1730868965.43 epoch sec; seqNo:2897 2024-11-06T04:56:05Z- AHRS_M2 msg: 0.09 phi; -0.08 theta; 3.50 psi 2024-11-06T04:56:05Z- handleDepth()- Depth_Keller msg: 1730868965.43 epoch sec; seqNo:3089; depth 48.72 2024-11-06T04:56:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868965.44 epoch sec; seqNo:3089; 4061691.96 north; 594891.89 east; 36.70 lat; -121.94 lon; 48.72 depth; utmZone: 10 2024-11-06T04:56:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868964.22 2024-11-06T04:56:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:05Z- waiting for 1730868968.22, time is 1730868965.44 2024-11-06T04:56:05Z- AHRS_M2 msg: 1730868965.83 epoch sec; seqNo:2898 2024-11-06T04:56:05Z- AHRS_M2 msg: 0.10 phi; -0.08 theta; 3.50 psi 2024-11-06T04:56:05Z- handleDepth()- Depth_Keller msg: 1730868965.84 epoch sec; seqNo:3090; depth 48.73 2024-11-06T04:56:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868965.85 epoch sec; seqNo:3090; 4061691.58 north; 594891.90 east; 36.70 lat; -121.94 lon; 48.73 depth; utmZone: 10 2024-11-06T04:56:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868964.22 2024-11-06T04:56:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:05Z- waiting for 1730868968.22, time is 1730868965.85 2024-11-06T04:56:06Z- AHRS_M2 msg: 1730868966.23 epoch sec; seqNo:2899 2024-11-06T04:56:06Z- AHRS_M2 msg: 0.10 phi; -0.08 theta; 3.50 psi 2024-11-06T04:56:06Z- handleDepth()- Depth_Keller msg: 1730868966.23 epoch sec; seqNo:3091; depth 48.74 2024-11-06T04:56:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868966.25 epoch sec; seqNo:3091; 4061691.20 north; 594891.90 east; 36.70 lat; -121.94 lon; 48.74 depth; utmZone: 10 2024-11-06T04:56:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868964.22 2024-11-06T04:56:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:06Z- waiting for 1730868968.22, time is 1730868966.25 2024-11-06T04:56:06Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:56:06Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:56:06Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:56:06Z- AHRS_M2 msg: 1730868966.63 epoch sec; seqNo:2900 2024-11-06T04:56:06Z- AHRS_M2 msg: 0.10 phi; -0.08 theta; 3.50 psi 2024-11-06T04:56:06Z- handleDepth()- Depth_Keller msg: 1730868966.66 epoch sec; seqNo:3092; depth 48.75 2024-11-06T04:56:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868966.70 epoch sec; seqNo:3092; 4061690.82 north; 594891.90 east; 36.70 lat; -121.94 lon; 48.75 depth; utmZone: 10 2024-11-06T04:56:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868964.22 2024-11-06T04:56:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:06Z- waiting for 1730868968.22, time is 1730868966.70 2024-11-06T04:56:07Z- AHRS_M2 msg: 1730868967.04 epoch sec; seqNo:2901 2024-11-06T04:56:07Z- AHRS_M2 msg: 0.11 phi; -0.08 theta; 3.50 psi 2024-11-06T04:56:07Z- handleDepth()- Depth_Keller msg: 1730868967.04 epoch sec; seqNo:3093; depth 48.75 2024-11-06T04:56:07Z- handleDVL() - DVL msg epochMillisec: 1730868967.06 2024-11-06T04:56:07Z- handleDvl() - RDI_Pathfinder msg: 1730868967.06 sec; seqNo:1114; #beams:4 2024-11-06T04:56:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.91 , 40.64 , 44.17 , 48.15 2024-11-06T04:56:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , -0.06 2024-11-06T04:56:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868967.08 epoch sec; seqNo:3093; 4061690.44 north; 594891.91 east; 36.70 lat; -121.94 lon; 48.75 depth; utmZone: 10 2024-11-06T04:56:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868964.22 2024-11-06T04:56:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:07Z- waiting for 1730868968.22, time is 1730868967.08 2024-11-06T04:56:07Z- AHRS_M2 msg: 1730868967.44 epoch sec; seqNo:2902 2024-11-06T04:56:07Z- AHRS_M2 msg: 0.11 phi; -0.08 theta; 3.50 psi 2024-11-06T04:56:07Z- handleDepth()- Depth_Keller msg: 1730868967.46 epoch sec; seqNo:3094; depth 48.75 2024-11-06T04:56:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868967.48 epoch sec; seqNo:3094; 4061690.06 north; 594891.91 east; 36.70 lat; -121.94 lon; 48.75 depth; utmZone: 10 2024-11-06T04:56:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868964.22 2024-11-06T04:56:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:07Z- waiting for 1730868968.22, time is 1730868967.48 2024-11-06T04:56:07Z- AHRS_M2 msg: 1730868967.84 epoch sec; seqNo:2903 2024-11-06T04:56:07Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 3.49 psi 2024-11-06T04:56:07Z- handleDepth()- Depth_Keller msg: 1730868967.85 epoch sec; seqNo:3095; depth 48.78 2024-11-06T04:56:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868967.86 epoch sec; seqNo:3095; 4061690.06 north; 594891.91 east; 36.70 lat; -121.94 lon; 48.78 depth; utmZone: 10 2024-11-06T04:56:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868964.22 2024-11-06T04:56:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:08Z- waiting for 1730868968.22, time is 1730868967.86 2024-11-06T04:56:08Z- AHRS_M2 msg: 1730868968.26 epoch sec; seqNo:2904 2024-11-06T04:56:08Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 3.49 psi 2024-11-06T04:56:08Z- handleDepth()- Depth_Keller msg: 1730868968.26 epoch sec; seqNo:3096; depth 48.76 2024-11-06T04:56:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868968.27 epoch sec; seqNo:3096; 4061689.67 north; 594890.70 east; 36.70 lat; -121.94 lon; 48.76 depth; utmZone: 10 2024-11-06T04:56:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868964.22 2024-11-06T04:56:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:08Z- waiting for 1730868968.22, time is 1730868968.27 2024-11-06T04:56:08Z- LcmTrn::updateTrn() >>>> pose time 1730868968.27, meas time 1730868967.06 <<<< 2024-11-06T04:56:08Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868968.27 2024-11-06T04:56:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868967.06 sec, ping # 0; waiting for INS data more recent than 1730868964.22... 2024-11-06T04:56:08Z- TerrainNav::motionUpdate() - currEstimate = (4061690, 594890.70, 48.76) 2024-11-06T04:56:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:56:08Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:56:08Z- TNavPF::Projecting Measurements 2024-11-06T04:56:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:56:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:56:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:56:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:56:08Z- TNavFilter::4 good beams. 2024-11-06T04:56:08Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:56:08Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:56:08Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:56:08Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:56:08Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:56:08Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:56:08Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:56:08Z- TerrainMapOctree: (vehN, vehE ) = (4061690.69, 594891.05). 2024-11-06T04:56:08Z- TerrainMapOctree: Min Distance = 153.01. 2024-11-06T04:56:08Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:56:08Z- Forcing Subcloud Comparison 2024-11-06T04:56:08Z- Weighting particles with subcloud comparison 2024-11-06T04:56:08Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:08Z- meanExpectedMeasDiff: 0.443435 delta_rms_squared: 2.879361 alpha: 0.993102 2024-11-06T04:56:08Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:08Z- meanExpectedMeasDiff: -2.678017 delta_rms_squared: 2.205148 alpha: 0.991012 2024-11-06T04:56:08Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:08Z- meanExpectedMeasDiff: 0.109955 delta_rms_squared: 2.282109 alpha: 0.991312 2024-11-06T04:56:08Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:08Z- meanExpectedMeasDiff: -0.385093 delta_rms_squared: 2.770775 alpha: 0.992834 2024-11-06T04:56:08Z- Subcloud NIS: 0.342472 2024-11-06T04:56:08Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:56:08Z- TNavPF:: sumWeights = 0.283799 2024-11-06T04:56:08Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:56:08Z- TNavPF::First Term of Map Covariance Matrix is 0.40 2024-11-06T04:56:08Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T04:56:08Z- TerrainNav::Resampling particle filter... 2024-11-06T04:56:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868967.06 sec. 2024-11-06T04:56:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:56:08Z- variances([0], [2], and psi_berg) = 0.608008 0.235645 0.000000 2024-11-06T04:56:08Z- publishEstimatesDecoder() - at 1730868968.27 sec 2024-11-06T04:56:08Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:56:08Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061634.67 594895.05 58.99 2024-11-06T04:56:08Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061633.13 594895.27 57.49 lat/lon: 36.70 -121.94 2024-11-06T04:56:08Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.61 0.24 0.85 0.00 2024-11-06T04:56:08Z- AHRS_M2 msg: 1730868968.65 epoch sec; seqNo:2905 2024-11-06T04:56:08Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 3.49 psi 2024-11-06T04:56:08Z- handleDepth()- Depth_Keller msg: 1730868968.66 epoch sec; seqNo:3097; depth 48.79 2024-11-06T04:56:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868968.74 epoch sec; seqNo:3097; 4061689.29 north; 594890.70 east; 36.70 lat; -121.94 lon; 48.79 depth; utmZone: 10 2024-11-06T04:56:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868968.27 2024-11-06T04:56:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:08Z- waiting for 1730868972.27, time is 1730868968.74 2024-11-06T04:56:09Z- handleDepth()- Depth_Keller msg: 1730868969.08 epoch sec; seqNo:3098; depth 48.81 2024-11-06T04:56:09Z- handleDVL() - DVL msg epochMillisec: 1730868969.10 2024-11-06T04:56:09Z- handleDvl() - RDI_Pathfinder msg: 1730868969.10 sec; seqNo:1115; #beams:4 2024-11-06T04:56:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.58 , 39.75 , 44.17 , 48.15 2024-11-06T04:56:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.13 , -0.03 2024-11-06T04:56:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868969.11 epoch sec; seqNo:3098; 4061688.91 north; 594890.70 east; 36.70 lat; -121.94 lon; 48.81 depth; utmZone: 10 2024-11-06T04:56:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868968.27 2024-11-06T04:56:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:09Z- waiting for 1730868972.27, time is 1730868969.11 2024-11-06T04:56:09Z- handleDepth()- Depth_Keller msg: 1730868969.48 epoch sec; seqNo:3099; depth 48.83 2024-11-06T04:56:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868969.50 epoch sec; seqNo:3099; 4061688.53 north; 594890.71 east; 36.70 lat; -121.94 lon; 48.83 depth; utmZone: 10 2024-11-06T04:56:09Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:56:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868968.27 2024-11-06T04:56:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:09Z- waiting for 1730868972.27, time is 1730868969.50 2024-11-06T04:56:09Z- AHRS_M2 msg: 1730868969.86 epoch sec; seqNo:2906 2024-11-06T04:56:09Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 3.47 psi 2024-11-06T04:56:09Z- handleDepth()- Depth_Keller msg: 1730868969.87 epoch sec; seqNo:3100; depth 48.84 2024-11-06T04:56:09Z- handleDVL() - DVL msg epochMillisec: 1730868969.87 2024-11-06T04:56:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:09Z- handleDvl() - RDI_Pathfinder msg: 1730868969.87 sec; seqNo:1116; #beams:0 2024-11-06T04:56:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.13 , -0.03 2024-11-06T04:56:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868969.89 epoch sec; seqNo:3100; 4061688.15 north; 594890.71 east; 36.70 lat; -121.94 lon; 48.84 depth; utmZone: 10 2024-11-06T04:56:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868968.27 2024-11-06T04:56:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:10Z- waiting for 1730868972.27, time is 1730868969.89 2024-11-06T04:56:10Z- AHRS_M2 msg: 1730868970.27 epoch sec; seqNo:2907 2024-11-06T04:56:10Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 3.47 psi 2024-11-06T04:56:10Z- handleDepth()- Depth_Keller msg: 1730868970.28 epoch sec; seqNo:3101; depth 48.86 2024-11-06T04:56:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868970.29 epoch sec; seqNo:3101; 4061687.78 north; 594890.72 east; 36.70 lat; -121.94 lon; 48.86 depth; utmZone: 10 2024-11-06T04:56:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868968.27 2024-11-06T04:56:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:10Z- waiting for 1730868972.27, time is 1730868970.29 2024-11-06T04:56:10Z- handleDepth()- Depth_Keller msg: 1730868970.72 epoch sec; seqNo:3102; depth 48.89 2024-11-06T04:56:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868970.75 epoch sec; seqNo:3102; 4061687.40 north; 594890.72 east; 36.70 lat; -121.94 lon; 48.89 depth; utmZone: 10 2024-11-06T04:56:10Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:56:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868968.27 2024-11-06T04:56:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:10Z- waiting for 1730868972.27, time is 1730868970.75 2024-11-06T04:56:11Z- handleDepth()- Depth_Keller msg: 1730868971.09 epoch sec; seqNo:3103; depth 48.91 2024-11-06T04:56:11Z- handleDVL() - DVL msg epochMillisec: 1730868971.15 2024-11-06T04:56:11Z- handleDvl() - RDI_Pathfinder msg: 1730868971.15 sec; seqNo:1117; #beams:4 2024-11-06T04:56:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.35 , 39.31 , 43.73 , 47.92 2024-11-06T04:56:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.19 , 0.05 2024-11-06T04:56:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868971.16 epoch sec; seqNo:3103; 4061687.00 north; 594889.51 east; 36.70 lat; -121.94 lon; 48.91 depth; utmZone: 10 2024-11-06T04:56:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868968.27 2024-11-06T04:56:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:11Z- waiting for 1730868972.27, time is 1730868971.16 2024-11-06T04:56:11Z- handleDepth()- Depth_Keller msg: 1730868971.51 epoch sec; seqNo:3104; depth 48.94 2024-11-06T04:56:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868971.53 epoch sec; seqNo:3104; 4061687.00 north; 594889.51 east; 36.70 lat; -121.94 lon; 48.94 depth; utmZone: 10 2024-11-06T04:56:11Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:56:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868968.27 2024-11-06T04:56:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:11Z- waiting for 1730868972.27, time is 1730868971.53 2024-11-06T04:56:11Z- handleDepth()- Depth_Keller msg: 1730868971.90 epoch sec; seqNo:3105; depth 48.98 2024-11-06T04:56:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868971.92 epoch sec; seqNo:3105; 4061686.63 north; 594889.51 east; 36.70 lat; -121.94 lon; 48.98 depth; utmZone: 10 2024-11-06T04:56:12Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:56:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868968.27 2024-11-06T04:56:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:12Z- waiting for 1730868972.27, time is 1730868971.92 2024-11-06T04:56:12Z- AHRS_M2 msg: 1730868972.29 epoch sec; seqNo:2908 2024-11-06T04:56:12Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 3.44 psi 2024-11-06T04:56:12Z- handleDepth()- Depth_Keller msg: 1730868972.29 epoch sec; seqNo:3106; depth 48.99 2024-11-06T04:56:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868972.31 epoch sec; seqNo:3106; 4061686.25 north; 594889.51 east; 36.70 lat; -121.94 lon; 48.99 depth; utmZone: 10 2024-11-06T04:56:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868968.27 2024-11-06T04:56:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:12Z- waiting for 1730868972.27, time is 1730868972.31 2024-11-06T04:56:12Z- LcmTrn::updateTrn() >>>> pose time 1730868972.31, meas time 1730868971.15 <<<< 2024-11-06T04:56:12Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868972.31 2024-11-06T04:56:12Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868971.15 sec, ping # 0; waiting for INS data more recent than 1730868968.27... 2024-11-06T04:56:12Z- TerrainNav::motionUpdate() - currEstimate = (4061686, 594889.51, 48.99) 2024-11-06T04:56:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:56:12Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:56:12Z- TNavPF::Projecting Measurements 2024-11-06T04:56:12Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:56:12Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:56:12Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:56:12Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:56:12Z- TNavFilter::4 good beams. 2024-11-06T04:56:12Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:56:12Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:56:12Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:56:12Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:56:12Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:56:12Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:56:12Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:56:12Z- TerrainMapOctree: (vehN, vehE ) = (4061687.23, 594889.85). 2024-11-06T04:56:12Z- TerrainMapOctree: Min Distance = 153.29. 2024-11-06T04:56:12Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:56:12Z- Forcing Subcloud Comparison 2024-11-06T04:56:12Z- Weighting particles with subcloud comparison 2024-11-06T04:56:12Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:12Z- meanExpectedMeasDiff: 1.385693 delta_rms_squared: 1.725239 alpha: 0.988540 2024-11-06T04:56:12Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:12Z- meanExpectedMeasDiff: -3.261931 delta_rms_squared: 1.301593 alpha: 0.984867 2024-11-06T04:56:12Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:12Z- meanExpectedMeasDiff: 0.260452 delta_rms_squared: 1.357293 alpha: 0.985479 2024-11-06T04:56:12Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:12Z- meanExpectedMeasDiff: 0.088106 delta_rms_squared: 1.645912 alpha: 0.987995 2024-11-06T04:56:12Z- Subcloud NIS: 0.333564 2024-11-06T04:56:12Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:56:12Z- TNavPF:: sumWeights = 0.172305 2024-11-06T04:56:12Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:56:12Z- TNavPF::First Term of Map Covariance Matrix is 0.47 2024-11-06T04:56:12Z- TNavPF::Calculated NIS Value : 0.09 numBeams normalized NIS: 0.02 2024-11-06T04:56:12Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868971.15 sec. 2024-11-06T04:56:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:56:12Z- variances([0], [2], and psi_berg) = 0.611101 0.237948 0.000000 2024-11-06T04:56:12Z- publishEstimatesDecoder() - at 1730868972.31 sec 2024-11-06T04:56:12Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:56:12Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061629.11 594894.14 57.82 2024-11-06T04:56:12Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061629.70 594894.09 57.95 lat/lon: 36.70 -121.94 2024-11-06T04:56:12Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.61 0.24 0.75 0.00 2024-11-06T04:56:12Z- AHRS_M2 msg: 1730868972.75 epoch sec; seqNo:2909 2024-11-06T04:56:12Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 3.43 psi 2024-11-06T04:56:12Z- handleDepth()- Depth_Keller msg: 1730868972.76 epoch sec; seqNo:3107; depth 49.02 2024-11-06T04:56:12Z- handleDVL() - DVL msg epochMillisec: 1730868972.78 2024-11-06T04:56:12Z- handleDvl() - RDI_Pathfinder msg: 1730868972.78 sec; seqNo:1118; #beams:4 2024-11-06T04:56:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.13 , 39.53 , 43.73 , 47.92 2024-11-06T04:56:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , 0.13 2024-11-06T04:56:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868972.80 epoch sec; seqNo:3107; 4061685.87 north; 594889.52 east; 36.70 lat; -121.94 lon; 49.02 depth; utmZone: 10 2024-11-06T04:56:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868972.31 2024-11-06T04:56:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:12Z- waiting for 1730868976.31, time is 1730868972.80 2024-11-06T04:56:13Z- AHRS_M2 msg: 1730868973.10 epoch sec; seqNo:2910 2024-11-06T04:56:13Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 3.43 psi 2024-11-06T04:56:13Z- handleDepth()- Depth_Keller msg: 1730868973.11 epoch sec; seqNo:3108; depth 49.05 2024-11-06T04:56:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868973.12 epoch sec; seqNo:3108; 4061685.49 north; 594889.52 east; 36.70 lat; -121.94 lon; 49.05 depth; utmZone: 10 2024-11-06T04:56:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868972.31 2024-11-06T04:56:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:13Z- waiting for 1730868976.31, time is 1730868973.12 2024-11-06T04:56:13Z- AHRS_M2 msg: 1730868973.50 epoch sec; seqNo:2911 2024-11-06T04:56:13Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 3.43 psi 2024-11-06T04:56:13Z- handleDepth()- Depth_Keller msg: 1730868973.53 epoch sec; seqNo:3109; depth 49.07 2024-11-06T04:56:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868973.55 epoch sec; seqNo:3109; 4061685.11 north; 594889.53 east; 36.70 lat; -121.94 lon; 49.07 depth; utmZone: 10 2024-11-06T04:56:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868972.31 2024-11-06T04:56:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:13Z- waiting for 1730868976.31, time is 1730868973.55 2024-11-06T04:56:13Z- AHRS_M2 msg: 1730868973.90 epoch sec; seqNo:2912 2024-11-06T04:56:13Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 3.43 psi 2024-11-06T04:56:13Z- handleDepth()- Depth_Keller msg: 1730868973.91 epoch sec; seqNo:3110; depth 49.10 2024-11-06T04:56:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868973.92 epoch sec; seqNo:3110; 4061685.11 north; 594889.53 east; 36.70 lat; -121.94 lon; 49.10 depth; utmZone: 10 2024-11-06T04:56:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868972.31 2024-11-06T04:56:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:14Z- waiting for 1730868976.31, time is 1730868973.92 2024-11-06T04:56:14Z- AHRS_M2 msg: 1730868974.31 epoch sec; seqNo:2913 2024-11-06T04:56:14Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 3.43 psi 2024-11-06T04:56:14Z- handleDepth()- Depth_Keller msg: 1730868974.31 epoch sec; seqNo:3111; depth 49.12 2024-11-06T04:56:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868974.32 epoch sec; seqNo:3111; 4061684.72 north; 594888.31 east; 36.70 lat; -121.94 lon; 49.12 depth; utmZone: 10 2024-11-06T04:56:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868972.31 2024-11-06T04:56:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:14Z- waiting for 1730868976.31, time is 1730868974.32 2024-11-06T04:56:14Z- AHRS_M2 msg: 1730868974.75 epoch sec; seqNo:2914 2024-11-06T04:56:14Z- AHRS_M2 msg: 0.13 phi; -0.08 theta; 3.43 psi 2024-11-06T04:56:14Z- handleDepth()- Depth_Keller msg: 1730868974.76 epoch sec; seqNo:3112; depth 49.13 2024-11-06T04:56:14Z- handleDVL() - DVL msg epochMillisec: 1730868974.78 2024-11-06T04:56:14Z- handleDvl() - RDI_Pathfinder msg: 1730868974.78 sec; seqNo:1119; #beams:4 2024-11-06T04:56:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.35 , 39.53 , 43.73 , 48.15 2024-11-06T04:56:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.12 2024-11-06T04:56:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868974.80 epoch sec; seqNo:3112; 4061684.34 north; 594888.31 east; 36.70 lat; -121.94 lon; 49.13 depth; utmZone: 10 2024-11-06T04:56:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868972.31 2024-11-06T04:56:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:14Z- waiting for 1730868976.31, time is 1730868974.80 2024-11-06T04:56:15Z- AHRS_M2 msg: 1730868975.12 epoch sec; seqNo:2915 2024-11-06T04:56:15Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 3.43 psi 2024-11-06T04:56:15Z- handleDepth()- Depth_Keller msg: 1730868975.12 epoch sec; seqNo:3113; depth 49.17 2024-11-06T04:56:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868975.14 epoch sec; seqNo:3113; 4061683.96 north; 594888.32 east; 36.70 lat; -121.94 lon; 49.17 depth; utmZone: 10 2024-11-06T04:56:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868972.31 2024-11-06T04:56:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:15Z- waiting for 1730868976.31, time is 1730868975.14 2024-11-06T04:56:15Z- AHRS_M2 msg: 1730868975.53 epoch sec; seqNo:2916 2024-11-06T04:56:15Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 3.43 psi 2024-11-06T04:56:15Z- handleDepth()- Depth_Keller msg: 1730868975.53 epoch sec; seqNo:3114; depth 49.19 2024-11-06T04:56:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868975.54 epoch sec; seqNo:3114; 4061683.58 north; 594888.32 east; 36.70 lat; -121.94 lon; 49.19 depth; utmZone: 10 2024-11-06T04:56:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868972.31 2024-11-06T04:56:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:15Z- waiting for 1730868976.31, time is 1730868975.54 2024-11-06T04:56:15Z- AHRS_M2 msg: 1730868975.92 epoch sec; seqNo:2917 2024-11-06T04:56:15Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 3.43 psi 2024-11-06T04:56:15Z- handleDepth()- Depth_Keller msg: 1730868975.93 epoch sec; seqNo:3115; depth 49.21 2024-11-06T04:56:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868975.94 epoch sec; seqNo:3115; 4061683.20 north; 594888.33 east; 36.70 lat; -121.94 lon; 49.21 depth; utmZone: 10 2024-11-06T04:56:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868972.31 2024-11-06T04:56:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:16Z- waiting for 1730868976.31, time is 1730868975.94 2024-11-06T04:56:16Z- AHRS_M2 msg: 1730868976.33 epoch sec; seqNo:2918 2024-11-06T04:56:16Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 3.43 psi 2024-11-06T04:56:16Z- handleDepth()- Depth_Keller msg: 1730868976.34 epoch sec; seqNo:3116; depth 49.23 2024-11-06T04:56:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868976.35 epoch sec; seqNo:3116; 4061682.82 north; 594888.33 east; 36.70 lat; -121.94 lon; 49.23 depth; utmZone: 10 2024-11-06T04:56:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868972.31 2024-11-06T04:56:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:16Z- waiting for 1730868976.31, time is 1730868976.35 2024-11-06T04:56:16Z- LcmTrn::updateTrn() >>>> pose time 1730868976.35, meas time 1730868974.78 <<<< 2024-11-06T04:56:16Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868976.35 2024-11-06T04:56:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868974.78 sec, ping # 0; waiting for INS data more recent than 1730868972.31... 2024-11-06T04:56:16Z- TerrainNav::motionUpdate() - currEstimate = (4061683, 594888.33, 49.23) 2024-11-06T04:56:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:56:16Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:56:16Z- TNavPF::Projecting Measurements 2024-11-06T04:56:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:56:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:56:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:56:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:56:16Z- TNavFilter::4 good beams. 2024-11-06T04:56:16Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:56:16Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:56:16Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:56:16Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:56:16Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:56:16Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:56:16Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:56:16Z- TerrainMapOctree: (vehN, vehE ) = (4061684.15, 594888.79). 2024-11-06T04:56:16Z- TerrainMapOctree: Min Distance = 153.62. 2024-11-06T04:56:16Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:56:16Z- Forcing Subcloud Comparison 2024-11-06T04:56:16Z- Weighting particles with subcloud comparison 2024-11-06T04:56:16Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:16Z- meanExpectedMeasDiff: 1.674147 delta_rms_squared: 1.586894 alpha: 0.987554 2024-11-06T04:56:16Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:16Z- meanExpectedMeasDiff: -2.453029 delta_rms_squared: 1.177871 alpha: 0.983304 2024-11-06T04:56:16Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:16Z- meanExpectedMeasDiff: 0.776484 delta_rms_squared: 1.233189 alpha: 0.984041 2024-11-06T04:56:16Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:16Z- meanExpectedMeasDiff: 0.702898 delta_rms_squared: 1.506721 alpha: 0.986900 2024-11-06T04:56:16Z- Subcloud NIS: 0.331766 2024-11-06T04:56:16Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:56:16Z- TNavPF:: sumWeights = 0.244996 2024-11-06T04:56:16Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:56:16Z- TNavPF::First Term of Map Covariance Matrix is 0.56 2024-11-06T04:56:16Z- TNavPF::Calculated NIS Value : 0.14 numBeams normalized NIS: 0.03 2024-11-06T04:56:16Z- TerrainNav::Resampling particle filter... 2024-11-06T04:56:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868974.78 sec. 2024-11-06T04:56:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:56:16Z- variances([0], [2], and psi_berg) = 0.614371 0.237568 0.000000 2024-11-06T04:56:16Z- publishEstimatesDecoder() - at 1730868976.35 sec 2024-11-06T04:56:16Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:56:16Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061627.79 594892.67 58.88 2024-11-06T04:56:16Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061626.27 594892.91 58.15 lat/lon: 36.70 -121.94 2024-11-06T04:56:16Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.61 0.24 0.74 0.00 2024-11-06T04:56:16Z- AHRS_M2 msg: 1730868976.77 epoch sec; seqNo:2919 2024-11-06T04:56:16Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 3.44 psi 2024-11-06T04:56:16Z- handleDepth()- Depth_Keller msg: 1730868976.78 epoch sec; seqNo:3117; depth 49.25 2024-11-06T04:56:16Z- handleDVL() - DVL msg epochMillisec: 1730868976.84 2024-11-06T04:56:16Z- handleDvl() - RDI_Pathfinder msg: 1730868976.84 sec; seqNo:1120; #beams:4 2024-11-06T04:56:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 51.02 , 39.31 , 43.95 , 48.59 2024-11-06T04:56:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.11 , 0.03 2024-11-06T04:56:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868976.87 epoch sec; seqNo:3117; 4061682.44 north; 594888.34 east; 36.70 lat; -121.94 lon; 49.25 depth; utmZone: 10 2024-11-06T04:56:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868976.35 2024-11-06T04:56:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:16Z- waiting for 1730868980.35, time is 1730868976.87 2024-11-06T04:56:17Z- AHRS_M2 msg: 1730868977.14 epoch sec; seqNo:2920 2024-11-06T04:56:17Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 3.44 psi 2024-11-06T04:56:17Z- handleDepth()- Depth_Keller msg: 1730868977.14 epoch sec; seqNo:3118; depth 49.28 2024-11-06T04:56:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868977.15 epoch sec; seqNo:3118; 4061682.07 north; 594888.34 east; 36.70 lat; -121.94 lon; 49.28 depth; utmZone: 10 2024-11-06T04:56:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868976.35 2024-11-06T04:56:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:17Z- waiting for 1730868980.35, time is 1730868977.15 2024-11-06T04:56:17Z- AHRS_M2 msg: 1730868977.54 epoch sec; seqNo:2921 2024-11-06T04:56:17Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 3.44 psi 2024-11-06T04:56:17Z- handleDepth()- Depth_Keller msg: 1730868977.56 epoch sec; seqNo:3119; depth 49.29 2024-11-06T04:56:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868977.57 epoch sec; seqNo:3119; 4061681.69 north; 594888.34 east; 36.70 lat; -121.94 lon; 49.29 depth; utmZone: 10 2024-11-06T04:56:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868976.35 2024-11-06T04:56:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:17Z- waiting for 1730868980.35, time is 1730868977.57 2024-11-06T04:56:17Z- AHRS_M2 msg: 1730868977.94 epoch sec; seqNo:2922 2024-11-06T04:56:17Z- AHRS_M2 msg: 0.12 phi; -0.09 theta; 3.44 psi 2024-11-06T04:56:17Z- handleDepth()- Depth_Keller msg: 1730868977.95 epoch sec; seqNo:3120; depth 49.31 2024-11-06T04:56:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868977.96 epoch sec; seqNo:3120; 4061681.31 north; 594888.35 east; 36.70 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T04:56:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868976.35 2024-11-06T04:56:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:18Z- waiting for 1730868980.35, time is 1730868977.96 2024-11-06T04:56:18Z- AHRS_M2 msg: 1730868978.35 epoch sec; seqNo:2923 2024-11-06T04:56:18Z- AHRS_M2 msg: 0.12 phi; -0.09 theta; 3.44 psi 2024-11-06T04:56:18Z- handleDepth()- Depth_Keller msg: 1730868978.35 epoch sec; seqNo:3121; depth 49.31 2024-11-06T04:56:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868978.36 epoch sec; seqNo:3121; 4061680.93 north; 594888.35 east; 36.70 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T04:56:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868976.35 2024-11-06T04:56:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:18Z- waiting for 1730868980.35, time is 1730868978.36 2024-11-06T04:56:18Z- AHRS_M2 msg: 1730868978.76 epoch sec; seqNo:2924 2024-11-06T04:56:18Z- AHRS_M2 msg: 0.12 phi; -0.09 theta; 3.44 psi 2024-11-06T04:56:18Z- handleDepth()- Depth_Keller msg: 1730868978.77 epoch sec; seqNo:3122; depth 49.33 2024-11-06T04:56:18Z- handleDVL() - DVL msg epochMillisec: 1730868978.79 2024-11-06T04:56:18Z- handleDvl() - RDI_Pathfinder msg: 1730868978.79 sec; seqNo:1121; #beams:4 2024-11-06T04:56:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.58 , 39.09 , 43.51 , 48.37 2024-11-06T04:56:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , -0.06 2024-11-06T04:56:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868978.80 epoch sec; seqNo:3122; 4061680.55 north; 594888.36 east; 36.70 lat; -121.94 lon; 49.33 depth; utmZone: 10 2024-11-06T04:56:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868976.35 2024-11-06T04:56:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:18Z- waiting for 1730868980.35, time is 1730868978.80 2024-11-06T04:56:19Z- AHRS_M2 msg: 1730868979.16 epoch sec; seqNo:2925 2024-11-06T04:56:19Z- AHRS_M2 msg: 0.12 phi; -0.09 theta; 3.43 psi 2024-11-06T04:56:19Z- handleDepth()- Depth_Keller msg: 1730868979.16 epoch sec; seqNo:3123; depth 49.35 2024-11-06T04:56:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868979.18 epoch sec; seqNo:3123; 4061680.17 north; 594888.36 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T04:56:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868976.35 2024-11-06T04:56:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:19Z- waiting for 1730868980.35, time is 1730868979.18 2024-11-06T04:56:19Z- AHRS_M2 msg: 1730868979.56 epoch sec; seqNo:2926 2024-11-06T04:56:19Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 3.43 psi 2024-11-06T04:56:19Z- handleDepth()- Depth_Keller msg: 1730868979.58 epoch sec; seqNo:3124; depth 49.35 2024-11-06T04:56:19Z- handleDVL() - DVL msg epochMillisec: 1730868979.58 2024-11-06T04:56:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:19Z- handleDvl() - RDI_Pathfinder msg: 1730868979.58 sec; seqNo:1122; #beams:0 2024-11-06T04:56:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , -0.06 2024-11-06T04:56:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868979.59 epoch sec; seqNo:3124; 4061679.79 north; 594888.36 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T04:56:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868976.35 2024-11-06T04:56:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:19Z- waiting for 1730868980.35, time is 1730868979.59 2024-11-06T04:56:19Z- AHRS_M2 msg: 1730868979.96 epoch sec; seqNo:2927 2024-11-06T04:56:19Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 3.43 psi 2024-11-06T04:56:19Z- handleDepth()- Depth_Keller msg: 1730868979.97 epoch sec; seqNo:3125; depth 49.35 2024-11-06T04:56:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868979.98 epoch sec; seqNo:3125; 4061679.41 north; 594888.37 east; 36.70 lat; -121.94 lon; 49.35 depth; utmZone: 10 2024-11-06T04:56:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868976.35 2024-11-06T04:56:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:20Z- waiting for 1730868980.35, time is 1730868979.98 2024-11-06T04:56:20Z- AHRS_M2 msg: 1730868980.37 epoch sec; seqNo:2928 2024-11-06T04:56:20Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 3.43 psi 2024-11-06T04:56:20Z- handleDepth()- Depth_Keller msg: 1730868980.38 epoch sec; seqNo:3126; depth 49.38 2024-11-06T04:56:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868980.39 epoch sec; seqNo:3126; 4061679.02 north; 594887.15 east; 36.70 lat; -121.94 lon; 49.38 depth; utmZone: 10 2024-11-06T04:56:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868976.35 2024-11-06T04:56:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:20Z- waiting for 1730868980.35, time is 1730868980.39 2024-11-06T04:56:20Z- LcmTrn::updateTrn() >>>> pose time 1730868980.39, meas time 1730868979.58 <<<< 2024-11-06T04:56:20Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868980.39 2024-11-06T04:56:20Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:56:20Z- TerrainNav::motionUpdate() - currEstimate = (4061679, 594887.15, 49.38) 2024-11-06T04:56:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:56:20Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:56:20Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:56:20Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:56:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:56:20Z- variances([0], [2], and psi_berg) = 0.617045 0.239576 0.000000 2024-11-06T04:56:20Z- publishEstimatesDecoder() - at 1730868980.39 sec 2024-11-06T04:56:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:56:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061623.88 594891.39 59.70 2024-11-06T04:56:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061622.47 594891.73 58.29 lat/lon: 36.70 -121.94 2024-11-06T04:56:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.62 0.24 0.99 0.00 2024-11-06T04:56:20Z- AHRS_M2 msg: 1730868980.77 epoch sec; seqNo:2929 2024-11-06T04:56:20Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 3.43 psi 2024-11-06T04:56:20Z- handleDepth()- Depth_Keller msg: 1730868980.78 epoch sec; seqNo:3127; depth 49.39 2024-11-06T04:56:20Z- handleDVL() - DVL msg epochMillisec: 1730868980.82 2024-11-06T04:56:20Z- handleDvl() - RDI_Pathfinder msg: 1730868980.82 sec; seqNo:1123; #beams:4 2024-11-06T04:56:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.58 , 38.87 , 43.29 , 48.15 2024-11-06T04:56:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.09 , -0.05 2024-11-06T04:56:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868980.83 epoch sec; seqNo:3127; 4061678.64 north; 594887.16 east; 36.70 lat; -121.94 lon; 49.39 depth; utmZone: 10 2024-11-06T04:56:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868980.39 2024-11-06T04:56:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:20Z- waiting for 1730868984.39, time is 1730868980.83 2024-11-06T04:56:21Z- AHRS_M2 msg: 1730868981.18 epoch sec; seqNo:2930 2024-11-06T04:56:21Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 3.42 psi 2024-11-06T04:56:21Z- handleDepth()- Depth_Keller msg: 1730868981.19 epoch sec; seqNo:3128; depth 49.40 2024-11-06T04:56:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868981.20 epoch sec; seqNo:3128; 4061678.26 north; 594887.16 east; 36.70 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T04:56:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868980.39 2024-11-06T04:56:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:21Z- waiting for 1730868984.39, time is 1730868981.20 2024-11-06T04:56:21Z- AHRS_M2 msg: 1730868981.58 epoch sec; seqNo:2931 2024-11-06T04:56:21Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 3.42 psi 2024-11-06T04:56:21Z- handleDepth()- Depth_Keller msg: 1730868981.60 epoch sec; seqNo:3129; depth 49.40 2024-11-06T04:56:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868981.61 epoch sec; seqNo:3129; 4061677.89 north; 594887.17 east; 36.70 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T04:56:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868980.39 2024-11-06T04:56:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:21Z- waiting for 1730868984.39, time is 1730868981.61 2024-11-06T04:56:21Z- AHRS_M2 msg: 1730868981.98 epoch sec; seqNo:2932 2024-11-06T04:56:21Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 3.42 psi 2024-11-06T04:56:21Z- handleDepth()- Depth_Keller msg: 1730868981.99 epoch sec; seqNo:3130; depth 49.42 2024-11-06T04:56:21Z- handleDVL() - DVL msg epochMillisec: 1730868981.99 2024-11-06T04:56:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:21Z- handleDvl() - RDI_Pathfinder msg: 1730868981.99 sec; seqNo:1124; #beams:0 2024-11-06T04:56:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.09 , -0.05 2024-11-06T04:56:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868982.01 epoch sec; seqNo:3130; 4061677.89 north; 594887.17 east; 36.70 lat; -121.94 lon; 49.42 depth; utmZone: 10 2024-11-06T04:56:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868980.39 2024-11-06T04:56:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:22Z- waiting for 1730868984.39, time is 1730868982.01 2024-11-06T04:56:22Z- AHRS_M2 msg: 1730868982.39 epoch sec; seqNo:2933 2024-11-06T04:56:22Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 3.41 psi 2024-11-06T04:56:22Z- handleDepth()- Depth_Keller msg: 1730868982.40 epoch sec; seqNo:3131; depth 49.44 2024-11-06T04:56:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868982.42 epoch sec; seqNo:3131; 4061677.51 north; 594887.17 east; 36.70 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T04:56:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868980.39 2024-11-06T04:56:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:22Z- waiting for 1730868984.39, time is 1730868982.42 2024-11-06T04:56:22Z- AHRS_M2 msg: 1730868982.79 epoch sec; seqNo:2934 2024-11-06T04:56:22Z- AHRS_M2 msg: 0.13 phi; -0.10 theta; 3.41 psi 2024-11-06T04:56:22Z- handleDepth()- Depth_Keller msg: 1730868982.80 epoch sec; seqNo:3132; depth 49.46 2024-11-06T04:56:22Z- handleDVL() - DVL msg epochMillisec: 1730868982.83 2024-11-06T04:56:22Z- handleDvl() - RDI_Pathfinder msg: 1730868982.83 sec; seqNo:1125; #beams:4 2024-11-06T04:56:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.35 , 38.74 , 42.81 , 48.32 2024-11-06T04:56:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.15 , 0.03 2024-11-06T04:56:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868982.85 epoch sec; seqNo:3132; 4061677.13 north; 594887.17 east; 36.70 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T04:56:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868980.39 2024-11-06T04:56:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:22Z- waiting for 1730868984.39, time is 1730868982.85 2024-11-06T04:56:23Z- AHRS_M2 msg: 1730868983.19 epoch sec; seqNo:2935 2024-11-06T04:56:23Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 3.40 psi 2024-11-06T04:56:23Z- handleDepth()- Depth_Keller msg: 1730868983.20 epoch sec; seqNo:3133; depth 49.50 2024-11-06T04:56:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868983.22 epoch sec; seqNo:3133; 4061676.75 north; 594887.18 east; 36.70 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T04:56:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868980.39 2024-11-06T04:56:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:23Z- waiting for 1730868984.39, time is 1730868983.22 2024-11-06T04:56:23Z- AHRS_M2 msg: 1730868983.60 epoch sec; seqNo:2936 2024-11-06T04:56:23Z- AHRS_M2 msg: 0.14 phi; -0.10 theta; 3.40 psi 2024-11-06T04:56:23Z- handleDepth()- Depth_Keller msg: 1730868983.61 epoch sec; seqNo:3134; depth 49.49 2024-11-06T04:56:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868983.63 epoch sec; seqNo:3134; 4061676.37 north; 594887.18 east; 36.70 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T04:56:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868980.39 2024-11-06T04:56:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:23Z- waiting for 1730868984.39, time is 1730868983.63 2024-11-06T04:56:24Z- AHRS_M2 msg: 1730868984.00 epoch sec; seqNo:2937 2024-11-06T04:56:24Z- AHRS_M2 msg: 0.13 phi; -0.10 theta; 3.39 psi 2024-11-06T04:56:24Z- handleDepth()- Depth_Keller msg: 1730868984.01 epoch sec; seqNo:3135; depth 49.51 2024-11-06T04:56:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868984.02 epoch sec; seqNo:3135; 4061675.98 north; 594885.97 east; 36.70 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T04:56:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868980.39 2024-11-06T04:56:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:24Z- waiting for 1730868984.39, time is 1730868984.02 2024-11-06T04:56:24Z- AHRS_M2 msg: 1730868984.41 epoch sec; seqNo:2938 2024-11-06T04:56:24Z- AHRS_M2 msg: 0.13 phi; -0.10 theta; 3.39 psi 2024-11-06T04:56:24Z- handleDepth()- Depth_Keller msg: 1730868984.41 epoch sec; seqNo:3136; depth 49.56 2024-11-06T04:56:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868984.42 epoch sec; seqNo:3136; 4061675.60 north; 594885.97 east; 36.70 lat; -121.94 lon; 49.56 depth; utmZone: 10 2024-11-06T04:56:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868980.39 2024-11-06T04:56:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:24Z- waiting for 1730868984.39, time is 1730868984.42 2024-11-06T04:56:24Z- LcmTrn::updateTrn() >>>> pose time 1730868984.42, meas time 1730868982.83 <<<< 2024-11-06T04:56:24Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868984.42 2024-11-06T04:56:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868982.83 sec, ping # 0; waiting for INS data more recent than 1730868980.39... 2024-11-06T04:56:24Z- TerrainNav::motionUpdate() - currEstimate = (4061676, 594885.97, 49.56) 2024-11-06T04:56:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:56:24Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:56:24Z- TNavPF::Projecting Measurements 2024-11-06T04:56:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:56:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:56:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:56:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:56:24Z- TNavFilter::4 good beams. 2024-11-06T04:56:24Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:56:24Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:56:24Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:56:24Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:56:24Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:56:24Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:56:24Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:56:24Z- TerrainMapOctree: (vehN, vehE ) = (4061676.95, 594886.44). 2024-11-06T04:56:24Z- TerrainMapOctree: Min Distance = 154.76. 2024-11-06T04:56:24Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:56:24Z- Forcing Subcloud Comparison 2024-11-06T04:56:24Z- Weighting particles with subcloud comparison 2024-11-06T04:56:24Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:24Z- meanExpectedMeasDiff: 1.887298 delta_rms_squared: 1.957186 alpha: 0.989885 2024-11-06T04:56:24Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:24Z- meanExpectedMeasDiff: -2.879766 delta_rms_squared: 1.440225 alpha: 0.986303 2024-11-06T04:56:24Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:24Z- meanExpectedMeasDiff: 0.388709 delta_rms_squared: 1.497125 alpha: 0.986817 2024-11-06T04:56:24Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:24Z- meanExpectedMeasDiff: 0.900774 delta_rms_squared: 1.872679 alpha: 0.989433 2024-11-06T04:56:24Z- Subcloud NIS: 0.335514 2024-11-06T04:56:24Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:56:24Z- TNavPF:: sumWeights = 0.163937 2024-11-06T04:56:24Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:56:24Z- TNavPF::First Term of Map Covariance Matrix is 0.56 2024-11-06T04:56:24Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T04:56:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868982.83 sec. 2024-11-06T04:56:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:56:24Z- variances([0], [2], and psi_berg) = 0.624787 0.243566 0.000000 2024-11-06T04:56:24Z- publishEstimatesDecoder() - at 1730868984.42 sec 2024-11-06T04:56:24Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:56:24Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061618.50 594890.32 58.56 2024-11-06T04:56:24Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061619.04 594890.55 58.49 lat/lon: 36.70 -121.94 2024-11-06T04:56:24Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.62 0.24 0.77 0.00 2024-11-06T04:56:24Z- AHRS_M2 msg: 1730868984.82 epoch sec; seqNo:2939 2024-11-06T04:56:24Z- AHRS_M2 msg: 0.13 phi; -0.10 theta; 3.39 psi 2024-11-06T04:56:24Z- handleDepth()- Depth_Keller msg: 1730868984.83 epoch sec; seqNo:3137; depth 49.58 2024-11-06T04:56:24Z- handleDVL() - DVL msg epochMillisec: 1730868984.85 2024-11-06T04:56:24Z- handleDvl() - RDI_Pathfinder msg: 1730868984.85 sec; seqNo:1126; #beams:4 2024-11-06T04:56:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.15 , 38.74 , 42.61 , 48.32 2024-11-06T04:56:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.09 , 0.11 2024-11-06T04:56:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868984.86 epoch sec; seqNo:3137; 4061675.22 north; 594885.97 east; 36.70 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T04:56:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868984.42 2024-11-06T04:56:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:24Z- waiting for 1730868988.42, time is 1730868984.86 2024-11-06T04:56:25Z- AHRS_M2 msg: 1730868985.21 epoch sec; seqNo:2940 2024-11-06T04:56:25Z- AHRS_M2 msg: 0.13 phi; -0.10 theta; 3.38 psi 2024-11-06T04:56:25Z- handleDepth()- Depth_Keller msg: 1730868985.22 epoch sec; seqNo:3138; depth 49.61 2024-11-06T04:56:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868985.24 epoch sec; seqNo:3138; 4061674.84 north; 594885.98 east; 36.70 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T04:56:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868984.42 2024-11-06T04:56:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:25Z- waiting for 1730868988.42, time is 1730868985.24 2024-11-06T04:56:25Z- AHRS_M2 msg: 1730868985.62 epoch sec; seqNo:2941 2024-11-06T04:56:25Z- AHRS_M2 msg: 0.13 phi; -0.10 theta; 3.38 psi 2024-11-06T04:56:25Z- handleDepth()- Depth_Keller msg: 1730868985.63 epoch sec; seqNo:3139; depth 49.63 2024-11-06T04:56:25Z- handleDVL() - DVL msg epochMillisec: 1730868985.63 2024-11-06T04:56:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:25Z- handleDvl() - RDI_Pathfinder msg: 1730868985.63 sec; seqNo:1127; #beams:0 2024-11-06T04:56:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.09 , 0.11 2024-11-06T04:56:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868985.64 epoch sec; seqNo:3139; 4061674.46 north; 594885.98 east; 36.70 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T04:56:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868984.42 2024-11-06T04:56:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:25Z- waiting for 1730868988.42, time is 1730868985.64 2024-11-06T04:56:26Z- AHRS_M2 msg: 1730868986.02 epoch sec; seqNo:2942 2024-11-06T04:56:26Z- AHRS_M2 msg: 0.12 phi; -0.10 theta; 3.38 psi 2024-11-06T04:56:26Z- handleDepth()- Depth_Keller msg: 1730868986.03 epoch sec; seqNo:3140; depth 49.64 2024-11-06T04:56:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868986.04 epoch sec; seqNo:3140; 4061674.46 north; 594885.98 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:56:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868984.42 2024-11-06T04:56:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:26Z- waiting for 1730868988.42, time is 1730868986.04 2024-11-06T04:56:26Z- AHRS_M2 msg: 1730868986.43 epoch sec; seqNo:2943 2024-11-06T04:56:26Z- AHRS_M2 msg: 0.12 phi; -0.10 theta; 3.38 psi 2024-11-06T04:56:26Z- handleDepth()- Depth_Keller msg: 1730868986.44 epoch sec; seqNo:3141; depth 49.67 2024-11-06T04:56:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868986.45 epoch sec; seqNo:3141; 4061674.08 north; 594885.99 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:56:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868984.42 2024-11-06T04:56:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:26Z- waiting for 1730868988.42, time is 1730868986.45 2024-11-06T04:56:26Z- handleDepth()- Depth_Keller msg: 1730868986.86 epoch sec; seqNo:3142; depth 49.70 2024-11-06T04:56:26Z- handleDVL() - DVL msg epochMillisec: 1730868986.88 2024-11-06T04:56:26Z- handleDvl() - RDI_Pathfinder msg: 1730868986.88 sec; seqNo:1128; #beams:4 2024-11-06T04:56:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.74 , 38.94 , 42.40 , 48.32 2024-11-06T04:56:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , 0.10 2024-11-06T04:56:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868986.90 epoch sec; seqNo:3142; 4061673.70 north; 594885.99 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T04:56:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868984.42 2024-11-06T04:56:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:27Z- waiting for 1730868988.42, time is 1730868986.90 2024-11-06T04:56:27Z- AHRS_M2 msg: 1730868987.24 epoch sec; seqNo:2944 2024-11-06T04:56:27Z- AHRS_M2 msg: 0.13 phi; -0.09 theta; 3.38 psi 2024-11-06T04:56:27Z- handleDepth()- Depth_Keller msg: 1730868987.24 epoch sec; seqNo:3143; depth 49.72 2024-11-06T04:56:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868987.26 epoch sec; seqNo:3143; 4061673.33 north; 594886.00 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:56:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868984.42 2024-11-06T04:56:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:27Z- waiting for 1730868988.42, time is 1730868987.26 2024-11-06T04:56:27Z- AHRS_M2 msg: 1730868987.64 epoch sec; seqNo:2945 2024-11-06T04:56:27Z- AHRS_M2 msg: 0.14 phi; -0.09 theta; 3.39 psi 2024-11-06T04:56:27Z- handleDepth()- Depth_Keller msg: 1730868987.65 epoch sec; seqNo:3144; depth 49.76 2024-11-06T04:56:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868987.66 epoch sec; seqNo:3144; 4061672.95 north; 594886.00 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T04:56:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868984.42 2024-11-06T04:56:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:27Z- waiting for 1730868988.42, time is 1730868987.66 2024-11-06T04:56:28Z- AHRS_M2 msg: 1730868988.05 epoch sec; seqNo:2946 2024-11-06T04:56:28Z- AHRS_M2 msg: 0.14 phi; -0.09 theta; 3.39 psi 2024-11-06T04:56:28Z- handleDepth()- Depth_Keller msg: 1730868988.05 epoch sec; seqNo:3145; depth 49.80 2024-11-06T04:56:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868988.06 epoch sec; seqNo:3145; 4061672.57 north; 594886.00 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T04:56:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868984.42 2024-11-06T04:56:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:28Z- waiting for 1730868988.42, time is 1730868988.06 2024-11-06T04:56:28Z- AHRS_M2 msg: 1730868988.45 epoch sec; seqNo:2947 2024-11-06T04:56:28Z- AHRS_M2 msg: 0.14 phi; -0.09 theta; 3.39 psi 2024-11-06T04:56:28Z- handleDepth()- Depth_Keller msg: 1730868988.46 epoch sec; seqNo:3146; depth 49.81 2024-11-06T04:56:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868988.47 epoch sec; seqNo:3146; 4061672.18 north; 594884.79 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:56:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868984.42 2024-11-06T04:56:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:28Z- waiting for 1730868988.42, time is 1730868988.47 2024-11-06T04:56:28Z- LcmTrn::updateTrn() >>>> pose time 1730868988.47, meas time 1730868986.88 <<<< 2024-11-06T04:56:28Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868988.47 2024-11-06T04:56:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868986.88 sec, ping # 0; waiting for INS data more recent than 1730868984.42... 2024-11-06T04:56:28Z- TerrainNav::motionUpdate() - currEstimate = (4061672, 594884.79, 49.81) 2024-11-06T04:56:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:56:28Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:56:28Z- TNavPF::Projecting Measurements 2024-11-06T04:56:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:56:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:56:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:56:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:56:28Z- TNavFilter::4 good beams. 2024-11-06T04:56:28Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:56:28Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:56:28Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:56:28Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:56:28Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:56:28Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:56:28Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:56:28Z- TerrainMapOctree: (vehN, vehE ) = (4061673.52, 594885.25). 2024-11-06T04:56:28Z- TerrainMapOctree: Min Distance = 155.38. 2024-11-06T04:56:28Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:56:28Z- Forcing Subcloud Comparison 2024-11-06T04:56:28Z- Weighting particles with subcloud comparison 2024-11-06T04:56:28Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:28Z- meanExpectedMeasDiff: 1.261307 delta_rms_squared: 1.640888 alpha: 0.987958 2024-11-06T04:56:28Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:28Z- meanExpectedMeasDiff: -2.265281 delta_rms_squared: 1.182410 alpha: 0.983367 2024-11-06T04:56:28Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:28Z- meanExpectedMeasDiff: 0.173125 delta_rms_squared: 1.242629 alpha: 0.984160 2024-11-06T04:56:28Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:28Z- meanExpectedMeasDiff: 1.183755 delta_rms_squared: 1.550265 alpha: 0.987263 2024-11-06T04:56:28Z- Subcloud NIS: 0.333062 2024-11-06T04:56:28Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:56:28Z- TNavPF:: sumWeights = 0.286865 2024-11-06T04:56:28Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:56:28Z- TNavPF::First Term of Map Covariance Matrix is 0.45 2024-11-06T04:56:28Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T04:56:28Z- TerrainNav::Resampling particle filter... 2024-11-06T04:56:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868986.88 sec. 2024-11-06T04:56:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:56:28Z- variances([0], [2], and psi_berg) = 0.624767 0.246459 0.000000 2024-11-06T04:56:28Z- publishEstimatesDecoder() - at 1730868988.47 sec 2024-11-06T04:56:28Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:56:28Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061617.02 594888.95 60.15 2024-11-06T04:56:28Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061615.62 594889.37 58.74 lat/lon: 36.70 -121.94 2024-11-06T04:56:28Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.62 0.25 0.74 0.00 2024-11-06T04:56:28Z- AHRS_M2 msg: 1730868988.85 epoch sec; seqNo:2948 2024-11-06T04:56:28Z- AHRS_M2 msg: 0.14 phi; -0.09 theta; 3.39 psi 2024-11-06T04:56:28Z- handleDepth()- Depth_Keller msg: 1730868988.87 epoch sec; seqNo:3147; depth 49.85 2024-11-06T04:56:28Z- handleDVL() - DVL msg epochMillisec: 1730868988.91 2024-11-06T04:56:28Z- handleDvl() - RDI_Pathfinder msg: 1730868988.91 sec; seqNo:1129; #beams:4 2024-11-06T04:56:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.56 , 38.74 , 42.81 , 47.91 2024-11-06T04:56:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , 0.02 2024-11-06T04:56:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868988.92 epoch sec; seqNo:3147; 4061671.80 north; 594884.79 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:56:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868988.47 2024-11-06T04:56:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:29Z- waiting for 1730868992.47, time is 1730868988.92 2024-11-06T04:56:29Z- AHRS_M2 msg: 1730868989.26 epoch sec; seqNo:2949 2024-11-06T04:56:29Z- AHRS_M2 msg: 0.14 phi; -0.09 theta; 3.39 psi 2024-11-06T04:56:29Z- handleDepth()- Depth_Keller msg: 1730868989.26 epoch sec; seqNo:3148; depth 49.86 2024-11-06T04:56:29Z- handleDVL() - DVL msg epochMillisec: 1730868989.27 2024-11-06T04:56:29Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:29Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:29Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:29Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:29Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:29Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:29Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:29Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:29Z- handleDvl() - RDI_Pathfinder msg: 1730868989.27 sec; seqNo:1130; #beams:0 2024-11-06T04:56:29Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , 0.02 2024-11-06T04:56:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868989.29 epoch sec; seqNo:3148; 4061671.42 north; 594884.80 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:56:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868988.47 2024-11-06T04:56:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:29Z- waiting for 1730868992.47, time is 1730868989.29 2024-11-06T04:56:29Z- AHRS_M2 msg: 1730868989.67 epoch sec; seqNo:2950 2024-11-06T04:56:29Z- AHRS_M2 msg: 0.15 phi; -0.09 theta; 3.39 psi 2024-11-06T04:56:29Z- handleDepth()- Depth_Keller msg: 1730868989.68 epoch sec; seqNo:3149; depth 49.89 2024-11-06T04:56:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868989.69 epoch sec; seqNo:3149; 4061671.04 north; 594884.80 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:56:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868988.47 2024-11-06T04:56:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:29Z- waiting for 1730868992.47, time is 1730868989.69 2024-11-06T04:56:30Z- handleDepth()- Depth_Keller msg: 1730868990.09 epoch sec; seqNo:3150; depth 49.91 2024-11-06T04:56:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868990.10 epoch sec; seqNo:3150; 4061670.66 north; 594884.80 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:56:30Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:56:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868988.47 2024-11-06T04:56:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:30Z- waiting for 1730868992.47, time is 1730868990.10 2024-11-06T04:56:30Z- AHRS_M2 msg: 1730868990.47 epoch sec; seqNo:2951 2024-11-06T04:56:30Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.40 psi 2024-11-06T04:56:30Z- handleDepth()- Depth_Keller msg: 1730868990.48 epoch sec; seqNo:3151; depth 49.94 2024-11-06T04:56:30Z- handleDVL() - DVL msg epochMillisec: 1730868990.48 2024-11-06T04:56:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:30Z- handleDvl() - RDI_Pathfinder msg: 1730868990.48 sec; seqNo:1131; #beams:0 2024-11-06T04:56:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , 0.02 2024-11-06T04:56:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868990.50 epoch sec; seqNo:3151; 4061670.28 north; 594884.81 east; 36.70 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T04:56:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868988.47 2024-11-06T04:56:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:30Z- waiting for 1730868992.47, time is 1730868990.50 2024-11-06T04:56:30Z- AHRS_M2 msg: 1730868990.87 epoch sec; seqNo:2952 2024-11-06T04:56:30Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.40 psi 2024-11-06T04:56:30Z- handleDepth()- Depth_Keller msg: 1730868990.88 epoch sec; seqNo:3152; depth 49.96 2024-11-06T04:56:30Z- handleDVL() - DVL msg epochMillisec: 1730868990.90 2024-11-06T04:56:30Z- handleDvl() - RDI_Pathfinder msg: 1730868990.90 sec; seqNo:1132; #beams:4 2024-11-06T04:56:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.56 , 38.53 , 42.81 , 47.91 2024-11-06T04:56:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.02 2024-11-06T04:56:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868990.92 epoch sec; seqNo:3152; 4061669.90 north; 594884.81 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:56:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868988.47 2024-11-06T04:56:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:31Z- waiting for 1730868992.47, time is 1730868990.92 2024-11-06T04:56:31Z- AHRS_M2 msg: 1730868991.28 epoch sec; seqNo:2953 2024-11-06T04:56:31Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.40 psi 2024-11-06T04:56:31Z- handleDepth()- Depth_Keller msg: 1730868991.29 epoch sec; seqNo:3153; depth 49.98 2024-11-06T04:56:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868991.30 epoch sec; seqNo:3153; 4061669.52 north; 594884.82 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T04:56:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868988.47 2024-11-06T04:56:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:31Z- waiting for 1730868992.47, time is 1730868991.30 2024-11-06T04:56:31Z- AHRS_M2 msg: 1730868991.71 epoch sec; seqNo:2954 2024-11-06T04:56:31Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.40 psi 2024-11-06T04:56:31Z- handleDepth()- Depth_Keller msg: 1730868991.72 epoch sec; seqNo:3154; depth 49.99 2024-11-06T04:56:31Z- handleDVL() - DVL msg epochMillisec: 1730868991.72 2024-11-06T04:56:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:31Z- handleDvl() - RDI_Pathfinder msg: 1730868991.72 sec; seqNo:1133; #beams:0 2024-11-06T04:56:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , -0.02 2024-11-06T04:56:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868991.73 epoch sec; seqNo:3154; 4061669.14 north; 594884.82 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T04:56:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868988.47 2024-11-06T04:56:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:31Z- waiting for 1730868992.47, time is 1730868991.73 2024-11-06T04:56:32Z- AHRS_M2 msg: 1730868992.08 epoch sec; seqNo:2955 2024-11-06T04:56:32Z- AHRS_M2 msg: 0.15 phi; -0.08 theta; 3.40 psi 2024-11-06T04:56:32Z- handleDepth()- Depth_Keller msg: 1730868992.10 epoch sec; seqNo:3155; depth 50.01 2024-11-06T04:56:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868992.11 epoch sec; seqNo:3155; 4061668.77 north; 594884.83 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:56:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868988.47 2024-11-06T04:56:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:32Z- waiting for 1730868992.47, time is 1730868992.11 2024-11-06T04:56:32Z- AHRS_M2 msg: 1730868992.49 epoch sec; seqNo:2956 2024-11-06T04:56:32Z- AHRS_M2 msg: 0.16 phi; -0.08 theta; 3.40 psi 2024-11-06T04:56:32Z- handleDepth()- Depth_Keller msg: 1730868992.49 epoch sec; seqNo:3156; depth 50.03 2024-11-06T04:56:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868992.50 epoch sec; seqNo:3156; 4061668.77 north; 594884.83 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T04:56:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868988.47 2024-11-06T04:56:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:32Z- waiting for 1730868992.47, time is 1730868992.50 2024-11-06T04:56:32Z- LcmTrn::updateTrn() >>>> pose time 1730868992.50, meas time 1730868991.72 <<<< 2024-11-06T04:56:32Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868992.50 2024-11-06T04:56:32Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:56:32Z- TerrainNav::motionUpdate() - currEstimate = (4061669, 594884.83, 50.03) 2024-11-06T04:56:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:56:32Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:56:32Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:56:32Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:56:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:56:32Z- variances([0], [2], and psi_berg) = 0.626356 0.248404 0.000000 2024-11-06T04:56:32Z- publishEstimatesDecoder() - at 1730868992.50 sec 2024-11-06T04:56:32Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:56:32Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061613.63 594888.97 60.33 2024-11-06T04:56:32Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061612.21 594889.40 58.95 lat/lon: 36.70 -121.94 2024-11-06T04:56:32Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.63 0.25 1.00 0.00 2024-11-06T04:56:32Z- AHRS_M2 msg: 1730868992.89 epoch sec; seqNo:2957 2024-11-06T04:56:32Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.40 psi 2024-11-06T04:56:32Z- handleDepth()- Depth_Keller msg: 1730868992.90 epoch sec; seqNo:3157; depth 50.04 2024-11-06T04:56:32Z- handleDVL() - DVL msg epochMillisec: 1730868992.92 2024-11-06T04:56:32Z- handleDvl() - RDI_Pathfinder msg: 1730868992.92 sec; seqNo:1134; #beams:4 2024-11-06T04:56:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.56 , 38.53 , 42.61 , 47.71 2024-11-06T04:56:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.06 , 0.01 2024-11-06T04:56:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868992.94 epoch sec; seqNo:3157; 4061668.39 north; 594884.83 east; 36.70 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T04:56:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868992.50 2024-11-06T04:56:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:33Z- waiting for 1730868996.50, time is 1730868992.94 2024-11-06T04:56:33Z- AHRS_M2 msg: 1730868993.30 epoch sec; seqNo:2958 2024-11-06T04:56:33Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.40 psi 2024-11-06T04:56:33Z- handleDepth()- Depth_Keller msg: 1730868993.30 epoch sec; seqNo:3158; depth 50.07 2024-11-06T04:56:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868993.32 epoch sec; seqNo:3158; 4061668.01 north; 594884.83 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T04:56:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868992.50 2024-11-06T04:56:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:33Z- waiting for 1730868996.50, time is 1730868993.32 2024-11-06T04:56:33Z- AHRS_M2 msg: 1730868993.70 epoch sec; seqNo:2959 2024-11-06T04:56:33Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.39 psi 2024-11-06T04:56:33Z- handleDepth()- Depth_Keller msg: 1730868993.70 epoch sec; seqNo:3159; depth 50.09 2024-11-06T04:56:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868993.72 epoch sec; seqNo:3159; 4061667.62 north; 594883.62 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T04:56:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868992.50 2024-11-06T04:56:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:33Z- waiting for 1730868996.50, time is 1730868993.72 2024-11-06T04:56:34Z- AHRS_M2 msg: 1730868994.10 epoch sec; seqNo:2960 2024-11-06T04:56:34Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.39 psi 2024-11-06T04:56:34Z- handleDepth()- Depth_Keller msg: 1730868994.11 epoch sec; seqNo:3160; depth 50.12 2024-11-06T04:56:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868994.13 epoch sec; seqNo:3160; 4061667.24 north; 594883.62 east; 36.70 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T04:56:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868992.50 2024-11-06T04:56:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:34Z- waiting for 1730868996.50, time is 1730868994.13 2024-11-06T04:56:34Z- AHRS_M2 msg: 1730868994.51 epoch sec; seqNo:2961 2024-11-06T04:56:34Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.39 psi 2024-11-06T04:56:34Z- handleDepth()- Depth_Keller msg: 1730868994.52 epoch sec; seqNo:3161; depth 50.13 2024-11-06T04:56:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868994.53 epoch sec; seqNo:3161; 4061666.86 north; 594883.63 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T04:56:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868992.50 2024-11-06T04:56:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:34Z- waiting for 1730868996.50, time is 1730868994.53 2024-11-06T04:56:34Z- AHRS_M2 msg: 1730868994.91 epoch sec; seqNo:2962 2024-11-06T04:56:34Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.39 psi 2024-11-06T04:56:34Z- handleDepth()- Depth_Keller msg: 1730868994.92 epoch sec; seqNo:3162; depth 50.15 2024-11-06T04:56:34Z- handleDVL() - DVL msg epochMillisec: 1730868994.94 2024-11-06T04:56:34Z- handleDvl() - RDI_Pathfinder msg: 1730868994.94 sec; seqNo:1135; #beams:4 2024-11-06T04:56:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.76 , 38.33 , 42.81 , 47.30 2024-11-06T04:56:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , 0.06 2024-11-06T04:56:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868994.95 epoch sec; seqNo:3162; 4061666.48 north; 594883.63 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T04:56:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868992.50 2024-11-06T04:56:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:35Z- waiting for 1730868996.50, time is 1730868994.95 2024-11-06T04:56:35Z- AHRS_M2 msg: 1730868995.32 epoch sec; seqNo:2963 2024-11-06T04:56:35Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.39 psi 2024-11-06T04:56:35Z- handleDepth()- Depth_Keller msg: 1730868995.32 epoch sec; seqNo:3163; depth 50.17 2024-11-06T04:56:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868995.34 epoch sec; seqNo:3163; 4061666.10 north; 594883.63 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T04:56:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868992.50 2024-11-06T04:56:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:35Z- waiting for 1730868996.50, time is 1730868995.34 2024-11-06T04:56:35Z- AHRS_M2 msg: 1730868995.72 epoch sec; seqNo:2964 2024-11-06T04:56:35Z- AHRS_M2 msg: 0.16 phi; -0.07 theta; 3.39 psi 2024-11-06T04:56:35Z- handleDepth()- Depth_Keller msg: 1730868995.73 epoch sec; seqNo:3164; depth 50.18 2024-11-06T04:56:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868995.74 epoch sec; seqNo:3164; 4061665.72 north; 594883.64 east; 36.70 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T04:56:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868992.50 2024-11-06T04:56:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:35Z- waiting for 1730868996.50, time is 1730868995.74 2024-11-06T04:56:36Z- AHRS_M2 msg: 1730868996.14 epoch sec; seqNo:2965 2024-11-06T04:56:36Z- AHRS_M2 msg: 0.15 phi; -0.07 theta; 3.39 psi 2024-11-06T04:56:36Z- handleDepth()- Depth_Keller msg: 1730868996.14 epoch sec; seqNo:3165; depth 50.17 2024-11-06T04:56:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868996.16 epoch sec; seqNo:3165; 4061665.34 north; 594883.64 east; 36.70 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T04:56:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868992.50 2024-11-06T04:56:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:36Z- waiting for 1730868996.50, time is 1730868996.16 2024-11-06T04:56:36Z- AHRS_M2 msg: 1730868996.53 epoch sec; seqNo:2966 2024-11-06T04:56:36Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.39 psi 2024-11-06T04:56:36Z- handleDepth()- Depth_Keller msg: 1730868996.53 epoch sec; seqNo:3166; depth 50.19 2024-11-06T04:56:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868996.54 epoch sec; seqNo:3166; 4061664.96 north; 594883.65 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:56:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868992.50 2024-11-06T04:56:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:36Z- waiting for 1730868996.50, time is 1730868996.54 2024-11-06T04:56:36Z- LcmTrn::updateTrn() >>>> pose time 1730868996.54, meas time 1730868994.94 <<<< 2024-11-06T04:56:36Z- LcmTrn::updateTrn() measUpdate first last update time: 1730868996.54 2024-11-06T04:56:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730868994.94 sec, ping # 0; waiting for INS data more recent than 1730868992.50... 2024-11-06T04:56:36Z- TerrainNav::motionUpdate() - currEstimate = (4061665, 594883.65, 50.19) 2024-11-06T04:56:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:56:36Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:56:36Z- TNavPF::Projecting Measurements 2024-11-06T04:56:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:56:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:56:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:56:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:56:36Z- TNavFilter::4 good beams. 2024-11-06T04:56:36Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:56:36Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:56:36Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:56:36Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:56:36Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:56:36Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:56:36Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:56:36Z- TerrainMapOctree: (vehN, vehE ) = (4061666.47, 594884.11). 2024-11-06T04:56:36Z- TerrainMapOctree: Min Distance = 158.00. 2024-11-06T04:56:36Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:56:36Z- Forcing Subcloud Comparison 2024-11-06T04:56:36Z- Weighting particles with subcloud comparison 2024-11-06T04:56:36Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:36Z- meanExpectedMeasDiff: 2.324542 delta_rms_squared: 2.030405 alpha: 0.990246 2024-11-06T04:56:36Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:36Z- meanExpectedMeasDiff: -2.111049 delta_rms_squared: 1.412055 alpha: 0.986034 2024-11-06T04:56:36Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:36Z- meanExpectedMeasDiff: 0.436886 delta_rms_squared: 1.551556 alpha: 0.987274 2024-11-06T04:56:36Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:36Z- meanExpectedMeasDiff: 1.373159 delta_rms_squared: 1.824055 alpha: 0.989154 2024-11-06T04:56:36Z- Subcloud NIS: 0.339831 2024-11-06T04:56:36Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:56:36Z- TNavPF:: sumWeights = 0.203919 2024-11-06T04:56:36Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:56:36Z- TNavPF::First Term of Map Covariance Matrix is 0.74 2024-11-06T04:56:36Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.03 2024-11-06T04:56:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730868994.94 sec. 2024-11-06T04:56:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:56:36Z- variances([0], [2], and psi_berg) = 0.626645 0.249257 0.000000 2024-11-06T04:56:36Z- publishEstimatesDecoder() - at 1730868996.54 sec 2024-11-06T04:56:36Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:56:36Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061608.55 594889.16 58.57 2024-11-06T04:56:36Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061608.41 594888.22 58.92 lat/lon: 36.70 -121.94 2024-11-06T04:56:36Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.63 0.25 0.79 0.00 2024-11-06T04:56:36Z- AHRS_M2 msg: 1730868996.93 epoch sec; seqNo:2967 2024-11-06T04:56:36Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.39 psi 2024-11-06T04:56:36Z- handleDepth()- Depth_Keller msg: 1730868996.94 epoch sec; seqNo:3167; depth 50.21 2024-11-06T04:56:37Z- handleDVL() - DVL msg epochMillisec: 1730868996.99 2024-11-06T04:56:37Z- handleDvl() - RDI_Pathfinder msg: 1730868996.99 sec; seqNo:1136; #beams:4 2024-11-06T04:56:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.56 , 38.33 , 42.81 , 47.09 2024-11-06T04:56:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , 0.06 2024-11-06T04:56:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868997.01 epoch sec; seqNo:3167; 4061664.58 north; 594883.65 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T04:56:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868996.54 2024-11-06T04:56:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:37Z- waiting for 1730869000.54, time is 1730868997.01 2024-11-06T04:56:37Z- AHRS_M2 msg: 1730868997.34 epoch sec; seqNo:2968 2024-11-06T04:56:37Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.39 psi 2024-11-06T04:56:37Z- handleDepth()- Depth_Keller msg: 1730868997.34 epoch sec; seqNo:3168; depth 50.22 2024-11-06T04:56:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868997.36 epoch sec; seqNo:3168; 4061664.58 north; 594883.65 east; 36.70 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T04:56:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868996.54 2024-11-06T04:56:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:37Z- waiting for 1730869000.54, time is 1730868997.36 2024-11-06T04:56:37Z- AHRS_M2 msg: 1730868997.75 epoch sec; seqNo:2969 2024-11-06T04:56:37Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.39 psi 2024-11-06T04:56:37Z- handleDepth()- Depth_Keller msg: 1730868997.75 epoch sec; seqNo:3169; depth 50.23 2024-11-06T04:56:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868997.76 epoch sec; seqNo:3169; 4061664.21 north; 594883.66 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T04:56:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868996.54 2024-11-06T04:56:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:37Z- waiting for 1730869000.54, time is 1730868997.76 2024-11-06T04:56:38Z- AHRS_M2 msg: 1730868998.15 epoch sec; seqNo:2970 2024-11-06T04:56:38Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.39 psi 2024-11-06T04:56:38Z- handleDepth()- Depth_Keller msg: 1730868998.15 epoch sec; seqNo:3170; depth 50.24 2024-11-06T04:56:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868998.18 epoch sec; seqNo:3170; 4061663.83 north; 594883.66 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:56:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868996.54 2024-11-06T04:56:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:38Z- waiting for 1730869000.54, time is 1730868998.18 2024-11-06T04:56:38Z- AHRS_M2 msg: 1730868998.55 epoch sec; seqNo:2971 2024-11-06T04:56:38Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.39 psi 2024-11-06T04:56:38Z- handleDepth()- Depth_Keller msg: 1730868998.55 epoch sec; seqNo:3171; depth 50.24 2024-11-06T04:56:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868998.56 epoch sec; seqNo:3171; 4061663.43 north; 594882.44 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:56:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868996.54 2024-11-06T04:56:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:38Z- waiting for 1730869000.54, time is 1730868998.56 2024-11-06T04:56:38Z- AHRS_M2 msg: 1730868998.96 epoch sec; seqNo:2972 2024-11-06T04:56:38Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.39 psi 2024-11-06T04:56:38Z- handleDepth()- Depth_Keller msg: 1730868998.97 epoch sec; seqNo:3172; depth 50.25 2024-11-06T04:56:38Z- handleDVL() - DVL msg epochMillisec: 1730868998.99 2024-11-06T04:56:38Z- handleDvl() - RDI_Pathfinder msg: 1730868998.99 sec; seqNo:1137; #beams:4 2024-11-06T04:56:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.35 , 38.53 , 42.81 , 46.69 2024-11-06T04:56:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , 0.02 2024-11-06T04:56:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868999.02 epoch sec; seqNo:3172; 4061663.06 north; 594882.45 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T04:56:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868996.54 2024-11-06T04:56:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:39Z- waiting for 1730869000.54, time is 1730868999.02 2024-11-06T04:56:39Z- AHRS_M2 msg: 1730868999.36 epoch sec; seqNo:2973 2024-11-06T04:56:39Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.39 psi 2024-11-06T04:56:39Z- handleDepth()- Depth_Keller msg: 1730868999.36 epoch sec; seqNo:3173; depth 50.27 2024-11-06T04:56:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868999.39 epoch sec; seqNo:3173; 4061662.68 north; 594882.45 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T04:56:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868996.54 2024-11-06T04:56:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:39Z- waiting for 1730869000.54, time is 1730868999.39 2024-11-06T04:56:39Z- AHRS_M2 msg: 1730868999.76 epoch sec; seqNo:2974 2024-11-06T04:56:39Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.39 psi 2024-11-06T04:56:39Z- handleDepth()- Depth_Keller msg: 1730868999.77 epoch sec; seqNo:3174; depth 50.26 2024-11-06T04:56:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730868999.78 epoch sec; seqNo:3174; 4061662.30 north; 594882.46 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:56:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868996.54 2024-11-06T04:56:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:39Z- waiting for 1730869000.54, time is 1730868999.78 2024-11-06T04:56:40Z- AHRS_M2 msg: 1730869000.16 epoch sec; seqNo:2975 2024-11-06T04:56:40Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.39 psi 2024-11-06T04:56:40Z- handleDepth()- Depth_Keller msg: 1730869000.17 epoch sec; seqNo:3175; depth 50.26 2024-11-06T04:56:40Z- handleDVL() - DVL msg epochMillisec: 1730869000.17 2024-11-06T04:56:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:40Z- handleDvl() - RDI_Pathfinder msg: 1730869000.17 sec; seqNo:1138; #beams:0 2024-11-06T04:56:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , 0.02 2024-11-06T04:56:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869000.18 epoch sec; seqNo:3175; 4061661.92 north; 594882.46 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:56:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868996.54 2024-11-06T04:56:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:40Z- waiting for 1730869000.54, time is 1730869000.18 2024-11-06T04:56:40Z- AHRS_M2 msg: 1730869000.57 epoch sec; seqNo:2976 2024-11-06T04:56:40Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.39 psi 2024-11-06T04:56:40Z- handleDepth()- Depth_Keller msg: 1730869000.57 epoch sec; seqNo:3176; depth 50.28 2024-11-06T04:56:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869000.58 epoch sec; seqNo:3176; 4061661.54 north; 594882.46 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T04:56:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730868996.54 2024-11-06T04:56:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:40Z- waiting for 1730869000.54, time is 1730869000.58 2024-11-06T04:56:40Z- LcmTrn::updateTrn() >>>> pose time 1730869000.58, meas time 1730869000.17 <<<< 2024-11-06T04:56:40Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869000.58 2024-11-06T04:56:40Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:56:40Z- TerrainNav::motionUpdate() - currEstimate = (4061662, 594882.46, 50.28) 2024-11-06T04:56:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:56:40Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:56:40Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:56:40Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:56:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:56:40Z- variances([0], [2], and psi_berg) = 0.628849 0.251317 0.000000 2024-11-06T04:56:40Z- publishEstimatesDecoder() - at 1730869000.58 sec 2024-11-06T04:56:40Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:56:40Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061605.03 594887.97 58.06 2024-11-06T04:56:40Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061604.98 594887.04 59.02 lat/lon: 36.70 -121.94 2024-11-06T04:56:40Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.63 0.25 1.03 0.00 2024-11-06T04:56:40Z- AHRS_M2 msg: 1730869000.98 epoch sec; seqNo:2977 2024-11-06T04:56:40Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.40 psi 2024-11-06T04:56:40Z- handleDepth()- Depth_Keller msg: 1730869000.98 epoch sec; seqNo:3177; depth 50.28 2024-11-06T04:56:41Z- handleDVL() - DVL msg epochMillisec: 1730869001.01 2024-11-06T04:56:41Z- handleDvl() - RDI_Pathfinder msg: 1730869001.01 sec; seqNo:1139; #beams:4 2024-11-06T04:56:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.15 , 38.33 , 42.81 , 46.69 2024-11-06T04:56:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.01 2024-11-06T04:56:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869001.02 epoch sec; seqNo:3177; 4061661.16 north; 594882.47 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T04:56:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869000.58 2024-11-06T04:56:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:41Z- waiting for 1730869004.58, time is 1730869001.02 2024-11-06T04:56:41Z- AHRS_M2 msg: 1730869001.38 epoch sec; seqNo:2978 2024-11-06T04:56:41Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.40 psi 2024-11-06T04:56:41Z- handleDepth()- Depth_Keller msg: 1730869001.38 epoch sec; seqNo:3178; depth 50.27 2024-11-06T04:56:41Z- handleDVL() - DVL msg epochMillisec: 1730869001.39 2024-11-06T04:56:41Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:41Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:41Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:41Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:41Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:41Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:41Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:41Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:41Z- handleDvl() - RDI_Pathfinder msg: 1730869001.39 sec; seqNo:1140; #beams:0 2024-11-06T04:56:41Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.01 2024-11-06T04:56:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869001.40 epoch sec; seqNo:3178; 4061660.78 north; 594882.47 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T04:56:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869000.58 2024-11-06T04:56:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:41Z- waiting for 1730869004.58, time is 1730869001.40 2024-11-06T04:56:41Z- AHRS_M2 msg: 1730869001.78 epoch sec; seqNo:2979 2024-11-06T04:56:41Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.40 psi 2024-11-06T04:56:41Z- handleDepth()- Depth_Keller msg: 1730869001.79 epoch sec; seqNo:3179; depth 50.29 2024-11-06T04:56:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869001.82 epoch sec; seqNo:3179; 4061660.40 north; 594882.48 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:56:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869000.58 2024-11-06T04:56:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:41Z- waiting for 1730869004.58, time is 1730869001.82 2024-11-06T04:56:42Z- AHRS_M2 msg: 1730869002.18 epoch sec; seqNo:2980 2024-11-06T04:56:42Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.40 psi 2024-11-06T04:56:42Z- handleDepth()- Depth_Keller msg: 1730869002.19 epoch sec; seqNo:3180; depth 50.29 2024-11-06T04:56:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869002.20 epoch sec; seqNo:3180; 4061660.03 north; 594882.48 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:56:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869000.58 2024-11-06T04:56:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:42Z- waiting for 1730869004.58, time is 1730869002.20 2024-11-06T04:56:42Z- AHRS_M2 msg: 1730869002.59 epoch sec; seqNo:2981 2024-11-06T04:56:42Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.40 psi 2024-11-06T04:56:42Z- handleDepth()- Depth_Keller msg: 1730869002.59 epoch sec; seqNo:3181; depth 50.29 2024-11-06T04:56:42Z- handleDVL() - DVL msg epochMillisec: 1730869002.60 2024-11-06T04:56:42Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:42Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:42Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:42Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:42Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:42Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:42Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:42Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:42Z- handleDvl() - RDI_Pathfinder msg: 1730869002.60 sec; seqNo:1141; #beams:0 2024-11-06T04:56:42Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.02 , -0.01 2024-11-06T04:56:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869002.60 epoch sec; seqNo:3181; 4061659.65 north; 594882.49 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:56:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869000.58 2024-11-06T04:56:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:42Z- waiting for 1730869004.58, time is 1730869002.60 2024-11-06T04:56:43Z- AHRS_M2 msg: 1730869002.99 epoch sec; seqNo:2982 2024-11-06T04:56:43Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.40 psi 2024-11-06T04:56:43Z- handleDepth()- Depth_Keller msg: 1730869003.01 epoch sec; seqNo:3182; depth 50.31 2024-11-06T04:56:43Z- handleDVL() - DVL msg epochMillisec: 1730869003.03 2024-11-06T04:56:43Z- handleDvl() - RDI_Pathfinder msg: 1730869003.03 sec; seqNo:1142; #beams:4 2024-11-06T04:56:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.95 , 38.12 , 42.61 , 46.48 2024-11-06T04:56:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.08 , -0.02 2024-11-06T04:56:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869003.05 epoch sec; seqNo:3182; 4061659.27 north; 594882.49 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:56:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869000.58 2024-11-06T04:56:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:43Z- waiting for 1730869004.58, time is 1730869003.05 2024-11-06T04:56:43Z- handleDepth()- Depth_Keller msg: 1730869003.41 epoch sec; seqNo:3183; depth 50.32 2024-11-06T04:56:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869003.43 epoch sec; seqNo:3183; 4061658.88 north; 594881.27 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:56:43Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:56:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869000.58 2024-11-06T04:56:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:43Z- waiting for 1730869004.58, time is 1730869003.43 2024-11-06T04:56:43Z- AHRS_M2 msg: 1730869003.80 epoch sec; seqNo:2983 2024-11-06T04:56:43Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.40 psi 2024-11-06T04:56:43Z- handleDepth()- Depth_Keller msg: 1730869003.82 epoch sec; seqNo:3184; depth 50.32 2024-11-06T04:56:43Z- handleDVL() - DVL msg epochMillisec: 1730869003.82 2024-11-06T04:56:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:43Z- handleDvl() - RDI_Pathfinder msg: 1730869003.82 sec; seqNo:1143; #beams:0 2024-11-06T04:56:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.08 , -0.02 2024-11-06T04:56:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869003.84 epoch sec; seqNo:3184; 4061658.88 north; 594881.27 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:56:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869000.58 2024-11-06T04:56:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:43Z- waiting for 1730869004.58, time is 1730869003.84 2024-11-06T04:56:44Z- AHRS_M2 msg: 1730869004.20 epoch sec; seqNo:2984 2024-11-06T04:56:44Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.40 psi 2024-11-06T04:56:44Z- handleDepth()- Depth_Keller msg: 1730869004.21 epoch sec; seqNo:3185; depth 50.32 2024-11-06T04:56:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869004.23 epoch sec; seqNo:3185; 4061658.50 north; 594881.28 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:56:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869000.58 2024-11-06T04:56:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:44Z- waiting for 1730869004.58, time is 1730869004.23 2024-11-06T04:56:44Z- AHRS_M2 msg: 1730869004.61 epoch sec; seqNo:2985 2024-11-06T04:56:44Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.39 psi 2024-11-06T04:56:44Z- handleDepth()- Depth_Keller msg: 1730869004.62 epoch sec; seqNo:3186; depth 50.35 2024-11-06T04:56:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869004.63 epoch sec; seqNo:3186; 4061658.12 north; 594881.28 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:56:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869000.58 2024-11-06T04:56:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:44Z- waiting for 1730869004.58, time is 1730869004.63 2024-11-06T04:56:44Z- LcmTrn::updateTrn() >>>> pose time 1730869004.63, meas time 1730869003.82 <<<< 2024-11-06T04:56:44Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869004.63 2024-11-06T04:56:44Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:56:44Z- TerrainNav::motionUpdate() - currEstimate = (4061658, 594881.28, 50.35) 2024-11-06T04:56:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:56:44Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:56:44Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:56:44Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:56:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:56:44Z- variances([0], [2], and psi_berg) = 0.629531 0.254188 0.000000 2024-11-06T04:56:44Z- publishEstimatesDecoder() - at 1730869004.63 sec 2024-11-06T04:56:44Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:56:44Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061601.53 594886.84 57.90 2024-11-06T04:56:44Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061601.56 594885.86 59.09 lat/lon: 36.70 -121.94 2024-11-06T04:56:44Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.63 0.25 1.28 0.00 2024-11-06T04:56:45Z- AHRS_M2 msg: 1730869005.01 epoch sec; seqNo:2986 2024-11-06T04:56:45Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.39 psi 2024-11-06T04:56:45Z- handleDepth()- Depth_Keller msg: 1730869005.02 epoch sec; seqNo:3187; depth 50.36 2024-11-06T04:56:45Z- handleDVL() - DVL msg epochMillisec: 1730869005.04 2024-11-06T04:56:45Z- handleDvl() - RDI_Pathfinder msg: 1730869005.04 sec; seqNo:1144; #beams:4 2024-11-06T04:56:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.74 , 38.12 , 42.61 , 46.28 2024-11-06T04:56:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.13 , 0.02 2024-11-06T04:56:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869005.07 epoch sec; seqNo:3187; 4061657.74 north; 594881.29 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:56:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869004.63 2024-11-06T04:56:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:45Z- waiting for 1730869008.63, time is 1730869005.07 2024-11-06T04:56:45Z- AHRS_M2 msg: 1730869005.42 epoch sec; seqNo:2987 2024-11-06T04:56:45Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.39 psi 2024-11-06T04:56:45Z- handleDepth()- Depth_Keller msg: 1730869005.42 epoch sec; seqNo:3188; depth 50.37 2024-11-06T04:56:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869005.44 epoch sec; seqNo:3188; 4061657.36 north; 594881.29 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:56:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869004.63 2024-11-06T04:56:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:45Z- waiting for 1730869008.63, time is 1730869005.44 2024-11-06T04:56:45Z- AHRS_M2 msg: 1730869005.82 epoch sec; seqNo:2988 2024-11-06T04:56:45Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.39 psi 2024-11-06T04:56:45Z- handleDepth()- Depth_Keller msg: 1730869005.84 epoch sec; seqNo:3189; depth 50.38 2024-11-06T04:56:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869005.84 epoch sec; seqNo:3189; 4061656.98 north; 594881.29 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:56:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869004.63 2024-11-06T04:56:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:45Z- waiting for 1730869008.63, time is 1730869005.84 2024-11-06T04:56:46Z- AHRS_M2 msg: 1730869006.22 epoch sec; seqNo:2989 2024-11-06T04:56:46Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.38 psi 2024-11-06T04:56:46Z- handleDepth()- Depth_Keller msg: 1730869006.23 epoch sec; seqNo:3190; depth 50.39 2024-11-06T04:56:46Z- handleDVL() - DVL msg epochMillisec: 1730869006.23 2024-11-06T04:56:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:46Z- handleDvl() - RDI_Pathfinder msg: 1730869006.23 sec; seqNo:1145; #beams:0 2024-11-06T04:56:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.13 , 0.02 2024-11-06T04:56:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869006.26 epoch sec; seqNo:3190; 4061656.60 north; 594881.30 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:56:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869004.63 2024-11-06T04:56:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:46Z- waiting for 1730869008.63, time is 1730869006.26 2024-11-06T04:56:46Z- AHRS_M2 msg: 1730869006.63 epoch sec; seqNo:2990 2024-11-06T04:56:46Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.38 psi 2024-11-06T04:56:46Z- handleDepth()- Depth_Keller msg: 1730869006.63 epoch sec; seqNo:3191; depth 50.41 2024-11-06T04:56:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869006.65 epoch sec; seqNo:3191; 4061656.22 north; 594881.30 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T04:56:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869004.63 2024-11-06T04:56:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:46Z- waiting for 1730869008.63, time is 1730869006.65 2024-11-06T04:56:47Z- AHRS_M2 msg: 1730869007.04 epoch sec; seqNo:2991 2024-11-06T04:56:47Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.38 psi 2024-11-06T04:56:47Z- handleDepth()- Depth_Keller msg: 1730869007.05 epoch sec; seqNo:3192; depth 50.44 2024-11-06T04:56:47Z- handleDVL() - DVL msg epochMillisec: 1730869007.07 2024-11-06T04:56:47Z- handleDvl() - RDI_Pathfinder msg: 1730869007.07 sec; seqNo:1146; #beams:4 2024-11-06T04:56:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.95 , 37.72 , 42.61 , 46.28 2024-11-06T04:56:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.09 , 0.09 2024-11-06T04:56:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869007.10 epoch sec; seqNo:3192; 4061655.83 north; 594880.09 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T04:56:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869004.63 2024-11-06T04:56:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:47Z- waiting for 1730869008.63, time is 1730869007.10 2024-11-06T04:56:47Z- AHRS_M2 msg: 1730869007.44 epoch sec; seqNo:2992 2024-11-06T04:56:47Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.38 psi 2024-11-06T04:56:47Z- handleDepth()- Depth_Keller msg: 1730869007.46 epoch sec; seqNo:3193; depth 50.46 2024-11-06T04:56:47Z- handleDVL() - DVL msg epochMillisec: 1730869007.46 2024-11-06T04:56:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:47Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:47Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:47Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:47Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:47Z- handleDvl() - RDI_Pathfinder msg: 1730869007.46 sec; seqNo:1147; #beams:0 2024-11-06T04:56:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.09 , 0.09 2024-11-06T04:56:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869007.48 epoch sec; seqNo:3193; 4061655.45 north; 594880.09 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T04:56:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869004.63 2024-11-06T04:56:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:47Z- waiting for 1730869008.63, time is 1730869007.48 2024-11-06T04:56:47Z- AHRS_M2 msg: 1730869007.84 epoch sec; seqNo:2993 2024-11-06T04:56:47Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.38 psi 2024-11-06T04:56:47Z- handleDepth()- Depth_Keller msg: 1730869007.85 epoch sec; seqNo:3194; depth 50.46 2024-11-06T04:56:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869007.86 epoch sec; seqNo:3194; 4061655.45 north; 594880.09 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T04:56:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869004.63 2024-11-06T04:56:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:48Z- waiting for 1730869008.63, time is 1730869007.86 2024-11-06T04:56:48Z- AHRS_M2 msg: 1730869008.24 epoch sec; seqNo:2994 2024-11-06T04:56:48Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.37 psi 2024-11-06T04:56:48Z- handleDepth()- Depth_Keller msg: 1730869008.25 epoch sec; seqNo:3195; depth 50.47 2024-11-06T04:56:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869008.26 epoch sec; seqNo:3195; 4061655.07 north; 594880.10 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T04:56:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869004.63 2024-11-06T04:56:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:48Z- waiting for 1730869008.63, time is 1730869008.26 2024-11-06T04:56:48Z- handleDepth()- Depth_Keller msg: 1730869008.67 epoch sec; seqNo:3196; depth 50.48 2024-11-06T04:56:48Z- handleDVL() - DVL msg epochMillisec: 1730869008.68 2024-11-06T04:56:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:48Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:48Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:48Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:48Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:48Z- handleDvl() - RDI_Pathfinder msg: 1730869008.68 sec; seqNo:1148; #beams:0 2024-11-06T04:56:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.09 , 0.09 2024-11-06T04:56:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869008.72 epoch sec; seqNo:3196; 4061654.69 north; 594880.10 east; 36.70 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T04:56:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869004.63 2024-11-06T04:56:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:48Z- waiting for 1730869008.63, time is 1730869008.72 2024-11-06T04:56:48Z- LcmTrn::updateTrn() >>>> pose time 1730869008.72, meas time 1730869008.68 <<<< 2024-11-06T04:56:48Z- LcmTrn::updateTrn() equating pose 1730869008.68 and meas times 1730869008.68 2024-11-06T04:56:48Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869008.68 2024-11-06T04:56:48Z- TerrainNav::motionUpdate() - currEstimate = (4061655, 594880.10, 50.48) 2024-11-06T04:56:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:56:48Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:56:48Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:56:48Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:56:48Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:56:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:56:48Z- variances([0], [2], and psi_berg) = 0.632037 0.256417 0.000000 2024-11-06T04:56:48Z- publishEstimatesDecoder() - at 1730869008.68 sec 2024-11-06T04:56:48Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:56:48Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061598.08 594885.65 58.35 2024-11-06T04:56:48Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061598.14 594884.68 59.22 lat/lon: 36.70 -121.94 2024-11-06T04:56:48Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.63 0.26 1.54 0.00 2024-11-06T04:56:49Z- AHRS_M2 msg: 1730869009.05 epoch sec; seqNo:2995 2024-11-06T04:56:49Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.37 psi 2024-11-06T04:56:49Z- handleDepth()- Depth_Keller msg: 1730869009.06 epoch sec; seqNo:3197; depth 50.52 2024-11-06T04:56:49Z- handleDVL() - DVL msg epochMillisec: 1730869009.09 2024-11-06T04:56:49Z- handleDvl() - RDI_Pathfinder msg: 1730869009.09 sec; seqNo:1149; #beams:4 2024-11-06T04:56:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.54 , 38.33 , 42.61 , 46.07 2024-11-06T04:56:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , 0.12 2024-11-06T04:56:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869009.11 epoch sec; seqNo:3197; 4061654.32 north; 594880.10 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T04:56:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869008.68 2024-11-06T04:56:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:49Z- waiting for 1730869012.68, time is 1730869009.11 2024-11-06T04:56:49Z- AHRS_M2 msg: 1730869009.46 epoch sec; seqNo:2996 2024-11-06T04:56:49Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.38 psi 2024-11-06T04:56:49Z- handleDepth()- Depth_Keller msg: 1730869009.47 epoch sec; seqNo:3198; depth 50.52 2024-11-06T04:56:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869009.49 epoch sec; seqNo:3198; 4061653.94 north; 594880.11 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T04:56:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869008.68 2024-11-06T04:56:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:49Z- waiting for 1730869012.68, time is 1730869009.49 2024-11-06T04:56:49Z- AHRS_M2 msg: 1730869009.86 epoch sec; seqNo:2997 2024-11-06T04:56:49Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.38 psi 2024-11-06T04:56:49Z- handleDepth()- Depth_Keller msg: 1730869009.87 epoch sec; seqNo:3199; depth 50.53 2024-11-06T04:56:49Z- handleDVL() - DVL msg epochMillisec: 1730869009.87 2024-11-06T04:56:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:49Z- handleDvl() - RDI_Pathfinder msg: 1730869009.87 sec; seqNo:1150; #beams:0 2024-11-06T04:56:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , 0.12 2024-11-06T04:56:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869009.88 epoch sec; seqNo:3199; 4061653.56 north; 594880.11 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T04:56:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869008.68 2024-11-06T04:56:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:50Z- waiting for 1730869012.68, time is 1730869009.88 2024-11-06T04:56:50Z- AHRS_M2 msg: 1730869010.26 epoch sec; seqNo:2998 2024-11-06T04:56:50Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.38 psi 2024-11-06T04:56:50Z- handleDepth()- Depth_Keller msg: 1730869010.27 epoch sec; seqNo:3200; depth 50.54 2024-11-06T04:56:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869010.28 epoch sec; seqNo:3200; 4061653.18 north; 594880.12 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T04:56:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869008.68 2024-11-06T04:56:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:50Z- waiting for 1730869012.68, time is 1730869010.28 2024-11-06T04:56:50Z- AHRS_M2 msg: 1730869010.67 epoch sec; seqNo:2999 2024-11-06T04:56:50Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 3.38 psi 2024-11-06T04:56:50Z- handleDepth()- Depth_Keller msg: 1730869010.70 epoch sec; seqNo:3201; depth 50.55 2024-11-06T04:56:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869010.76 epoch sec; seqNo:3201; 4061652.80 north; 594880.12 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:56:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869008.68 2024-11-06T04:56:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:50Z- waiting for 1730869012.68, time is 1730869010.76 2024-11-06T04:56:51Z- AHRS_M2 msg: 1730869011.07 epoch sec; seqNo:3000 2024-11-06T04:56:51Z- AHRS_M2 msg: 0.14 phi; -0.04 theta; 3.39 psi 2024-11-06T04:56:51Z- handleDepth()- Depth_Keller msg: 1730869011.08 epoch sec; seqNo:3202; depth 50.55 2024-11-06T04:56:51Z- handleDVL() - DVL msg epochMillisec: 1730869011.11 2024-11-06T04:56:51Z- handleDvl() - RDI_Pathfinder msg: 1730869011.11 sec; seqNo:1151; #beams:4 2024-11-06T04:56:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.74 , 38.53 , 42.61 , 45.87 2024-11-06T04:56:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.09 , 0.05 2024-11-06T04:56:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869011.17 epoch sec; seqNo:3202; 4061652.42 north; 594880.12 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:56:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869008.68 2024-11-06T04:56:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:51Z- waiting for 1730869012.68, time is 1730869011.17 2024-11-06T04:56:51Z- AHRS_M2 msg: 1730869011.47 epoch sec; seqNo:3001 2024-11-06T04:56:51Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.39 psi 2024-11-06T04:56:51Z- handleDepth()- Depth_Keller msg: 1730869011.48 epoch sec; seqNo:3203; depth 50.55 2024-11-06T04:56:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869011.49 epoch sec; seqNo:3203; 4061652.04 north; 594880.13 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:56:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869008.68 2024-11-06T04:56:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:51Z- waiting for 1730869012.68, time is 1730869011.49 2024-11-06T04:56:51Z- AHRS_M2 msg: 1730869011.88 epoch sec; seqNo:3002 2024-11-06T04:56:51Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.39 psi 2024-11-06T04:56:51Z- handleDepth()- Depth_Keller msg: 1730869011.88 epoch sec; seqNo:3204; depth 50.57 2024-11-06T04:56:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869011.89 epoch sec; seqNo:3204; 4061651.66 north; 594880.13 east; 36.70 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T04:56:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869008.68 2024-11-06T04:56:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:52Z- waiting for 1730869012.68, time is 1730869011.89 2024-11-06T04:56:52Z- AHRS_M2 msg: 1730869012.28 epoch sec; seqNo:3003 2024-11-06T04:56:52Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.40 psi 2024-11-06T04:56:52Z- handleDepth()- Depth_Keller msg: 1730869012.29 epoch sec; seqNo:3205; depth 50.55 2024-11-06T04:56:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869012.31 epoch sec; seqNo:3205; 4061651.28 north; 594880.14 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:56:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869008.68 2024-11-06T04:56:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:52Z- waiting for 1730869012.68, time is 1730869012.31 2024-11-06T04:56:52Z- AHRS_M2 msg: 1730869012.75 epoch sec; seqNo:3004 2024-11-06T04:56:52Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.40 psi 2024-11-06T04:56:52Z- handleDepth()- Depth_Keller msg: 1730869012.75 epoch sec; seqNo:3206; depth 50.56 2024-11-06T04:56:52Z- handleDVL() - DVL msg epochMillisec: 1730869012.78 2024-11-06T04:56:52Z- handleDvl() - RDI_Pathfinder msg: 1730869012.78 sec; seqNo:1152; #beams:4 2024-11-06T04:56:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.16 , 38.53 , 42.82 , 45.67 2024-11-06T04:56:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , -0.05 2024-11-06T04:56:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869012.79 epoch sec; seqNo:3206; 4061650.91 north; 594880.14 east; 36.70 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T04:56:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869008.68 2024-11-06T04:56:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:52Z- waiting for 1730869012.68, time is 1730869012.79 2024-11-06T04:56:52Z- LcmTrn::updateTrn() >>>> pose time 1730869012.79, meas time 1730869012.78 <<<< 2024-11-06T04:56:52Z- LcmTrn::updateTrn() equating pose 1730869012.78 and meas times 1730869012.78 2024-11-06T04:56:52Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869012.78 2024-11-06T04:56:52Z- TerrainNav::motionUpdate() - currEstimate = (4061651, 594880.14, 50.56) 2024-11-06T04:56:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:56:52Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:56:52Z- TNavPF::Projecting Measurements 2024-11-06T04:56:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:56:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:56:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:56:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:56:52Z- TNavFilter::4 good beams. 2024-11-06T04:56:52Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:56:52Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:56:52Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:56:52Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:56:52Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:56:52Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:56:52Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:56:52Z- TerrainMapOctree: (vehN, vehE ) = (4061650.91, 594880.14). 2024-11-06T04:56:52Z- TerrainMapOctree: Min Distance = 163.57. 2024-11-06T04:56:52Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:56:53Z- Forcing Subcloud Comparison 2024-11-06T04:56:53Z- Weighting particles with subcloud comparison 2024-11-06T04:56:53Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:53Z- meanExpectedMeasDiff: 2.113836 delta_rms_squared: 2.915124 alpha: 0.993186 2024-11-06T04:56:53Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:53Z- meanExpectedMeasDiff: -1.549304 delta_rms_squared: 2.004763 alpha: 0.990122 2024-11-06T04:56:53Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:53Z- meanExpectedMeasDiff: -0.097327 delta_rms_squared: 2.323908 alpha: 0.991467 2024-11-06T04:56:53Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:53Z- meanExpectedMeasDiff: 1.135098 delta_rms_squared: 2.503737 alpha: 0.992075 2024-11-06T04:56:53Z- Subcloud NIS: 0.340270 2024-11-06T04:56:53Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:56:53Z- TNavPF:: sumWeights = 0.267934 2024-11-06T04:56:53Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:56:53Z- TNavPF::First Term of Map Covariance Matrix is 0.77 2024-11-06T04:56:53Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T04:56:53Z- TerrainNav::Resampling particle filter... 2024-11-06T04:56:53Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869012.78 sec, ping # 0. 2024-11-06T04:56:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:56:53Z- variances([0], [2], and psi_berg) = 0.643332 0.263096 0.000000 2024-11-06T04:56:53Z- publishEstimatesDecoder() - at 1730869012.78 sec 2024-11-06T04:56:53Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:56:53Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061596.30 594884.45 59.57 2024-11-06T04:56:53Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061594.35 594884.71 59.11 lat/lon: 36.70 -121.94 2024-11-06T04:56:53Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.64 0.26 0.59 0.00 2024-11-06T04:56:53Z- AHRS_M2 msg: 1730869013.12 epoch sec; seqNo:3005 2024-11-06T04:56:53Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.40 psi 2024-11-06T04:56:53Z- handleDepth()- Depth_Keller msg: 1730869013.12 epoch sec; seqNo:3207; depth 50.55 2024-11-06T04:56:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869013.15 epoch sec; seqNo:3207; 4061650.53 north; 594880.15 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:56:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869012.78 2024-11-06T04:56:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:53Z- waiting for 1730869016.78, time is 1730869013.15 2024-11-06T04:56:53Z- AHRS_M2 msg: 1730869013.49 epoch sec; seqNo:3006 2024-11-06T04:56:53Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.40 psi 2024-11-06T04:56:53Z- handleDepth()- Depth_Keller msg: 1730869013.50 epoch sec; seqNo:3208; depth 50.54 2024-11-06T04:56:53Z- handleDVL() - DVL msg epochMillisec: 1730869013.51 2024-11-06T04:56:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:53Z- handleDvl() - RDI_Pathfinder msg: 1730869013.51 sec; seqNo:1153; #beams:0 2024-11-06T04:56:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , -0.05 2024-11-06T04:56:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869013.52 epoch sec; seqNo:3208; 4061650.13 north; 594878.93 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T04:56:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869012.78 2024-11-06T04:56:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:53Z- waiting for 1730869016.78, time is 1730869013.52 2024-11-06T04:56:53Z- AHRS_M2 msg: 1730869013.90 epoch sec; seqNo:3007 2024-11-06T04:56:53Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.40 psi 2024-11-06T04:56:53Z- handleDepth()- Depth_Keller msg: 1730869013.90 epoch sec; seqNo:3209; depth 50.52 2024-11-06T04:56:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869013.91 epoch sec; seqNo:3209; 4061649.76 north; 594878.93 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T04:56:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869012.78 2024-11-06T04:56:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:54Z- waiting for 1730869016.78, time is 1730869013.91 2024-11-06T04:56:54Z- AHRS_M2 msg: 1730869014.31 epoch sec; seqNo:3008 2024-11-06T04:56:54Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.40 psi 2024-11-06T04:56:54Z- handleDepth()- Depth_Keller msg: 1730869014.31 epoch sec; seqNo:3210; depth 50.51 2024-11-06T04:56:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869014.32 epoch sec; seqNo:3210; 4061649.38 north; 594878.94 east; 36.70 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T04:56:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869012.78 2024-11-06T04:56:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:54Z- waiting for 1730869016.78, time is 1730869014.32 2024-11-06T04:56:54Z- AHRS_M2 msg: 1730869014.76 epoch sec; seqNo:3009 2024-11-06T04:56:54Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.40 psi 2024-11-06T04:56:54Z- handleDepth()- Depth_Keller msg: 1730869014.76 epoch sec; seqNo:3211; depth 50.52 2024-11-06T04:56:54Z- handleDVL() - DVL msg epochMillisec: 1730869014.78 2024-11-06T04:56:54Z- handleDvl() - RDI_Pathfinder msg: 1730869014.78 sec; seqNo:1154; #beams:4 2024-11-06T04:56:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.95 , 38.33 , 42.81 , 45.46 2024-11-06T04:56:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.08 2024-11-06T04:56:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869014.79 epoch sec; seqNo:3211; 4061649.00 north; 594878.94 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T04:56:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869012.78 2024-11-06T04:56:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:54Z- waiting for 1730869016.78, time is 1730869014.79 2024-11-06T04:56:55Z- AHRS_M2 msg: 1730869015.11 epoch sec; seqNo:3010 2024-11-06T04:56:55Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.40 psi 2024-11-06T04:56:55Z- handleDepth()- Depth_Keller msg: 1730869015.12 epoch sec; seqNo:3212; depth 50.49 2024-11-06T04:56:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869015.14 epoch sec; seqNo:3212; 4061649.00 north; 594878.94 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T04:56:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869012.78 2024-11-06T04:56:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:55Z- waiting for 1730869016.78, time is 1730869015.14 2024-11-06T04:56:55Z- AHRS_M2 msg: 1730869015.52 epoch sec; seqNo:3011 2024-11-06T04:56:55Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.40 psi 2024-11-06T04:56:55Z- handleDepth()- Depth_Keller msg: 1730869015.53 epoch sec; seqNo:3213; depth 50.49 2024-11-06T04:56:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869015.54 epoch sec; seqNo:3213; 4061648.62 north; 594878.95 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T04:56:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869012.78 2024-11-06T04:56:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:55Z- waiting for 1730869016.78, time is 1730869015.54 2024-11-06T04:56:55Z- AHRS_M2 msg: 1730869015.92 epoch sec; seqNo:3012 2024-11-06T04:56:55Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.39 psi 2024-11-06T04:56:55Z- handleDepth()- Depth_Keller msg: 1730869015.93 epoch sec; seqNo:3214; depth 50.46 2024-11-06T04:56:55Z- handleDVL() - DVL msg epochMillisec: 1730869015.93 2024-11-06T04:56:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:55Z- handleDvl() - RDI_Pathfinder msg: 1730869015.93 sec; seqNo:1155; #beams:0 2024-11-06T04:56:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.08 2024-11-06T04:56:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869015.94 epoch sec; seqNo:3214; 4061648.24 north; 594878.95 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T04:56:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869012.78 2024-11-06T04:56:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:56Z- waiting for 1730869016.78, time is 1730869015.94 2024-11-06T04:56:56Z- AHRS_M2 msg: 1730869016.33 epoch sec; seqNo:3013 2024-11-06T04:56:56Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.39 psi 2024-11-06T04:56:56Z- handleDepth()- Depth_Keller msg: 1730869016.34 epoch sec; seqNo:3215; depth 50.46 2024-11-06T04:56:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869016.34 epoch sec; seqNo:3215; 4061647.86 north; 594878.95 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T04:56:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869012.78 2024-11-06T04:56:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:56Z- waiting for 1730869016.78, time is 1730869016.34 2024-11-06T04:56:56Z- AHRS_M2 msg: 1730869016.76 epoch sec; seqNo:3014 2024-11-06T04:56:56Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.39 psi 2024-11-06T04:56:56Z- handleDepth()- Depth_Keller msg: 1730869016.76 epoch sec; seqNo:3216; depth 50.45 2024-11-06T04:56:56Z- handleDVL() - DVL msg epochMillisec: 1730869016.78 2024-11-06T04:56:56Z- handleDvl() - RDI_Pathfinder msg: 1730869016.78 sec; seqNo:1156; #beams:4 2024-11-06T04:56:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.34 , 38.12 , 42.81 , 44.85 2024-11-06T04:56:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.14 , -0.00 2024-11-06T04:56:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869016.81 epoch sec; seqNo:3216; 4061647.48 north; 594878.96 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T04:56:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869012.78 2024-11-06T04:56:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:56Z- waiting for 1730869016.78, time is 1730869016.81 2024-11-06T04:56:56Z- LcmTrn::updateTrn() >>>> pose time 1730869016.81, meas time 1730869016.78 <<<< 2024-11-06T04:56:56Z- LcmTrn::updateTrn() equating pose 1730869016.78 and meas times 1730869016.78 2024-11-06T04:56:56Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869016.78 2024-11-06T04:56:56Z- TerrainNav::motionUpdate() - currEstimate = (4061647, 594878.96, 50.45) 2024-11-06T04:56:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:56:56Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:56:56Z- TNavPF::Projecting Measurements 2024-11-06T04:56:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:56:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:56:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:56:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:56:56Z- TNavFilter::4 good beams. 2024-11-06T04:56:56Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:56:56Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:56:56Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:56:56Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:56:56Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:56:56Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:56:56Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:56:56Z- TerrainMapOctree: (vehN, vehE ) = (4061647.48, 594878.96). 2024-11-06T04:56:56Z- TerrainMapOctree: Min Distance = 164.74. 2024-11-06T04:56:56Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:56:57Z- Forcing Subcloud Comparison 2024-11-06T04:56:57Z- Weighting particles with subcloud comparison 2024-11-06T04:56:57Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:57Z- meanExpectedMeasDiff: 1.454975 delta_rms_squared: 1.368112 alpha: 0.985592 2024-11-06T04:56:57Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:57Z- meanExpectedMeasDiff: -2.071451 delta_rms_squared: 0.830100 alpha: 0.976473 2024-11-06T04:56:57Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:57Z- meanExpectedMeasDiff: -0.713722 delta_rms_squared: 1.128805 alpha: 0.982591 2024-11-06T04:56:57Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:56:57Z- meanExpectedMeasDiff: 0.483730 delta_rms_squared: 1.173268 alpha: 0.983239 2024-11-06T04:56:57Z- Subcloud NIS: 0.328666 2024-11-06T04:56:57Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:56:57Z- TNavPF:: sumWeights = 0.335752 2024-11-06T04:56:57Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:56:57Z- TNavPF::First Term of Map Covariance Matrix is 0.61 2024-11-06T04:56:57Z- TNavPF::Calculated NIS Value : 0.20 numBeams normalized NIS: 0.05 2024-11-06T04:56:57Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869016.78 sec, ping # 0. 2024-11-06T04:56:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:56:57Z- variances([0], [2], and psi_berg) = 0.686986 0.275432 0.000000 2024-11-06T04:56:57Z- publishEstimatesDecoder() - at 1730869016.78 sec 2024-11-06T04:56:57Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:56:57Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061589.19 594883.82 59.10 2024-11-06T04:56:57Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061590.89 594883.45 59.08 lat/lon: 36.70 -121.94 2024-11-06T04:56:57Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.69 0.28 0.47 0.00 2024-11-06T04:56:57Z- AHRS_M2 msg: 1730869017.14 epoch sec; seqNo:3015 2024-11-06T04:56:57Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T04:56:57Z- handleDepth()- Depth_Keller msg: 1730869017.15 epoch sec; seqNo:3217; depth 50.44 2024-11-06T04:56:57Z- handleDVL() - DVL msg epochMillisec: 1730869017.15 2024-11-06T04:56:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:57Z- handleDvl() - RDI_Pathfinder msg: 1730869017.15 sec; seqNo:1157; #beams:0 2024-11-06T04:56:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.14 , -0.00 2024-11-06T04:56:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869017.18 epoch sec; seqNo:3217; 4061647.10 north; 594878.96 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T04:56:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869016.78 2024-11-06T04:56:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:57Z- waiting for 1730869020.78, time is 1730869017.18 2024-11-06T04:56:57Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:56:57Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:56:57Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:56:57Z- AHRS_M2 msg: 1730869017.53 epoch sec; seqNo:3016 2024-11-06T04:56:57Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T04:56:57Z- handleDepth()- Depth_Keller msg: 1730869017.56 epoch sec; seqNo:3218; depth 50.44 2024-11-06T04:56:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869017.57 epoch sec; seqNo:3218; 4061646.71 north; 594877.75 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T04:56:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869016.78 2024-11-06T04:56:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:57Z- waiting for 1730869020.78, time is 1730869017.57 2024-11-06T04:56:57Z- AHRS_M2 msg: 1730869017.94 epoch sec; seqNo:3017 2024-11-06T04:56:57Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.39 psi 2024-11-06T04:56:57Z- handleDepth()- Depth_Keller msg: 1730869017.96 epoch sec; seqNo:3219; depth 50.44 2024-11-06T04:56:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869017.97 epoch sec; seqNo:3219; 4061646.33 north; 594877.75 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T04:56:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869016.78 2024-11-06T04:56:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:58Z- waiting for 1730869020.78, time is 1730869017.97 2024-11-06T04:56:58Z- AHRS_M2 msg: 1730869018.34 epoch sec; seqNo:3018 2024-11-06T04:56:58Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.39 psi 2024-11-06T04:56:58Z- handleDepth()- Depth_Keller msg: 1730869018.35 epoch sec; seqNo:3220; depth 50.44 2024-11-06T04:56:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869018.36 epoch sec; seqNo:3220; 4061645.95 north; 594877.76 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T04:56:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869016.78 2024-11-06T04:56:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:58Z- waiting for 1730869020.78, time is 1730869018.36 2024-11-06T04:56:58Z- AHRS_M2 msg: 1730869018.77 epoch sec; seqNo:3019 2024-11-06T04:56:58Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.39 psi 2024-11-06T04:56:58Z- handleDepth()- Depth_Keller msg: 1730869018.77 epoch sec; seqNo:3221; depth 50.44 2024-11-06T04:56:58Z- handleDVL() - DVL msg epochMillisec: 1730869018.80 2024-11-06T04:56:58Z- handleDvl() - RDI_Pathfinder msg: 1730869018.80 sec; seqNo:1158; #beams:4 2024-11-06T04:56:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.93 , 38.13 , 42.61 , 44.24 2024-11-06T04:56:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.14 , 0.07 2024-11-06T04:56:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869018.81 epoch sec; seqNo:3221; 4061645.58 north; 594877.76 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T04:56:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869016.78 2024-11-06T04:56:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:58Z- waiting for 1730869020.78, time is 1730869018.81 2024-11-06T04:56:59Z- AHRS_M2 msg: 1730869019.16 epoch sec; seqNo:3020 2024-11-06T04:56:59Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.39 psi 2024-11-06T04:56:59Z- handleDepth()- Depth_Keller msg: 1730869019.17 epoch sec; seqNo:3222; depth 50.44 2024-11-06T04:56:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869019.19 epoch sec; seqNo:3222; 4061645.58 north; 594877.76 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T04:56:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:56:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869016.78 2024-11-06T04:56:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:59Z- waiting for 1730869020.78, time is 1730869019.19 2024-11-06T04:56:59Z- AHRS_M2 msg: 1730869019.56 epoch sec; seqNo:3021 2024-11-06T04:56:59Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.38 psi 2024-11-06T04:56:59Z- handleDepth()- Depth_Keller msg: 1730869019.56 epoch sec; seqNo:3223; depth 50.44 2024-11-06T04:56:59Z- handleDVL() - DVL msg epochMillisec: 1730869019.57 2024-11-06T04:56:59Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:59Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:59Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:56:59Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:56:59Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:56:59Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:56:59Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:56:59Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:56:59Z- handleDvl() - RDI_Pathfinder msg: 1730869019.57 sec; seqNo:1159; #beams:0 2024-11-06T04:56:59Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:56:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.14 , 0.07 2024-11-06T04:56:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869019.58 epoch sec; seqNo:3223; 4061645.20 north; 594877.76 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T04:56:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:56:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869016.78 2024-11-06T04:56:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:56:59Z- waiting for 1730869020.78, time is 1730869019.58 2024-11-06T04:56:59Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:56:59Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:56:59Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:56:59Z- AHRS_M2 msg: 1730869019.95 epoch sec; seqNo:3022 2024-11-06T04:56:59Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.38 psi 2024-11-06T04:56:59Z- handleDepth()- Depth_Keller msg: 1730869019.98 epoch sec; seqNo:3224; depth 50.47 2024-11-06T04:57:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869019.99 epoch sec; seqNo:3224; 4061644.82 north; 594877.77 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T04:57:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869016.78 2024-11-06T04:57:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:00Z- waiting for 1730869020.78, time is 1730869019.99 2024-11-06T04:57:00Z- AHRS_M2 msg: 1730869020.36 epoch sec; seqNo:3023 2024-11-06T04:57:00Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.38 psi 2024-11-06T04:57:00Z- handleDepth()- Depth_Keller msg: 1730869020.37 epoch sec; seqNo:3225; depth 50.47 2024-11-06T04:57:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869020.38 epoch sec; seqNo:3225; 4061644.44 north; 594877.77 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T04:57:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869016.78 2024-11-06T04:57:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:00Z- waiting for 1730869020.78, time is 1730869020.38 2024-11-06T04:57:00Z- AHRS_M2 msg: 1730869020.77 epoch sec; seqNo:3024 2024-11-06T04:57:00Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.38 psi 2024-11-06T04:57:00Z- handleDepth()- Depth_Keller msg: 1730869020.78 epoch sec; seqNo:3226; depth 50.46 2024-11-06T04:57:00Z- handleDVL() - DVL msg epochMillisec: 1730869020.82 2024-11-06T04:57:00Z- handleDvl() - RDI_Pathfinder msg: 1730869020.82 sec; seqNo:1160; #beams:4 2024-11-06T04:57:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.54 , 38.13 , 43.02 , 44.45 2024-11-06T04:57:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , 0.11 2024-11-06T04:57:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869020.84 epoch sec; seqNo:3226; 4061644.06 north; 594877.78 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T04:57:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869016.78 2024-11-06T04:57:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:00Z- waiting for 1730869020.78, time is 1730869020.84 2024-11-06T04:57:00Z- LcmTrn::updateTrn() >>>> pose time 1730869020.84, meas time 1730869020.82 <<<< 2024-11-06T04:57:00Z- LcmTrn::updateTrn() equating pose 1730869020.82 and meas times 1730869020.82 2024-11-06T04:57:00Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869020.82 2024-11-06T04:57:00Z- TerrainNav::motionUpdate() - currEstimate = (4061644, 594877.78, 50.46) 2024-11-06T04:57:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:57:00Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:57:00Z- TNavPF::Projecting Measurements 2024-11-06T04:57:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:57:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:57:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:57:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:57:00Z- TNavFilter::4 good beams. 2024-11-06T04:57:00Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:57:00Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:57:00Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:57:00Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:57:00Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:57:00Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:57:00Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:57:00Z- TerrainMapOctree: (vehN, vehE ) = (4061644.06, 594877.78). 2024-11-06T04:57:00Z- TerrainMapOctree: Min Distance = 165.98. 2024-11-06T04:57:00Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:57:01Z- Forcing Subcloud Comparison 2024-11-06T04:57:01Z- Weighting particles with subcloud comparison 2024-11-06T04:57:01Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:01Z- meanExpectedMeasDiff: 1.447577 delta_rms_squared: 1.173937 alpha: 0.983249 2024-11-06T04:57:01Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:01Z- meanExpectedMeasDiff: -1.851266 delta_rms_squared: 0.644140 alpha: 0.969886 2024-11-06T04:57:01Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:01Z- meanExpectedMeasDiff: -0.840860 delta_rms_squared: 0.974595 alpha: 0.979891 2024-11-06T04:57:01Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:01Z- meanExpectedMeasDiff: 0.531294 delta_rms_squared: 0.977058 alpha: 0.979941 2024-11-06T04:57:01Z- Subcloud NIS: 0.323516 2024-11-06T04:57:01Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:57:01Z- TNavPF:: sumWeights = 0.378904 2024-11-06T04:57:01Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:57:01Z- TNavPF::First Term of Map Covariance Matrix is 0.59 2024-11-06T04:57:01Z- TNavPF::Calculated NIS Value : 0.24 numBeams normalized NIS: 0.06 2024-11-06T04:57:01Z- TerrainNav::Resampling particle filter... 2024-11-06T04:57:01Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869020.82 sec, ping # 0. 2024-11-06T04:57:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:57:01Z- variances([0], [2], and psi_berg) = 0.710155 0.287955 0.000000 2024-11-06T04:57:01Z- publishEstimatesDecoder() - at 1730869020.82 sec 2024-11-06T04:57:01Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:57:01Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061589.43 594882.09 58.67 2024-11-06T04:57:01Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061587.48 594882.19 59.14 lat/lon: 36.70 -121.94 2024-11-06T04:57:01Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.71 0.29 0.42 0.00 2024-11-06T04:57:01Z- AHRS_M2 msg: 1730869021.17 epoch sec; seqNo:3025 2024-11-06T04:57:01Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.38 psi 2024-11-06T04:57:01Z- handleDepth()- Depth_Keller msg: 1730869021.19 epoch sec; seqNo:3227; depth 50.47 2024-11-06T04:57:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869021.22 epoch sec; seqNo:3227; 4061643.68 north; 594877.78 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T04:57:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869020.82 2024-11-06T04:57:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:01Z- waiting for 1730869024.82, time is 1730869021.22 2024-11-06T04:57:01Z- AHRS_M2 msg: 1730869021.58 epoch sec; seqNo:3026 2024-11-06T04:57:01Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 3.38 psi 2024-11-06T04:57:01Z- handleDepth()- Depth_Keller msg: 1730869021.58 epoch sec; seqNo:3228; depth 50.46 2024-11-06T04:57:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869021.59 epoch sec; seqNo:3228; 4061643.29 north; 594876.56 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T04:57:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869020.82 2024-11-06T04:57:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:01Z- waiting for 1730869024.82, time is 1730869021.59 2024-11-06T04:57:01Z- AHRS_M2 msg: 1730869021.98 epoch sec; seqNo:3027 2024-11-06T04:57:01Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 3.38 psi 2024-11-06T04:57:01Z- handleDepth()- Depth_Keller msg: 1730869021.99 epoch sec; seqNo:3229; depth 50.46 2024-11-06T04:57:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869022.00 epoch sec; seqNo:3229; 4061642.91 north; 594876.57 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T04:57:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869020.82 2024-11-06T04:57:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:02Z- waiting for 1730869024.82, time is 1730869022.00 2024-11-06T04:57:02Z- AHRS_M2 msg: 1730869022.38 epoch sec; seqNo:3028 2024-11-06T04:57:02Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 3.38 psi 2024-11-06T04:57:02Z- handleDepth()- Depth_Keller msg: 1730869022.39 epoch sec; seqNo:3230; depth 50.47 2024-11-06T04:57:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869022.40 epoch sec; seqNo:3230; 4061642.53 north; 594876.57 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T04:57:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869020.82 2024-11-06T04:57:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:02Z- waiting for 1730869024.82, time is 1730869022.40 2024-11-06T04:57:02Z- AHRS_M2 msg: 1730869022.79 epoch sec; seqNo:3029 2024-11-06T04:57:02Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 3.38 psi 2024-11-06T04:57:02Z- handleDepth()- Depth_Keller msg: 1730869022.80 epoch sec; seqNo:3231; depth 50.47 2024-11-06T04:57:02Z- handleDVL() - DVL msg epochMillisec: 1730869022.83 2024-11-06T04:57:02Z- handleDvl() - RDI_Pathfinder msg: 1730869022.83 sec; seqNo:1161; #beams:4 2024-11-06T04:57:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.93 , 37.92 , 43.02 , 44.03 2024-11-06T04:57:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.06 , 0.08 2024-11-06T04:57:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869022.86 epoch sec; seqNo:3231; 4061642.15 north; 594876.58 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T04:57:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869020.82 2024-11-06T04:57:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:02Z- waiting for 1730869024.82, time is 1730869022.86 2024-11-06T04:57:03Z- AHRS_M2 msg: 1730869023.19 epoch sec; seqNo:3030 2024-11-06T04:57:03Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.38 psi 2024-11-06T04:57:03Z- handleDepth()- Depth_Keller msg: 1730869023.20 epoch sec; seqNo:3232; depth 50.46 2024-11-06T04:57:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869023.23 epoch sec; seqNo:3232; 4061641.77 north; 594876.58 east; 36.70 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T04:57:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869020.82 2024-11-06T04:57:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:03Z- waiting for 1730869024.82, time is 1730869023.23 2024-11-06T04:57:03Z- AHRS_M2 msg: 1730869023.60 epoch sec; seqNo:3031 2024-11-06T04:57:03Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.38 psi 2024-11-06T04:57:03Z- handleDepth()- Depth_Keller msg: 1730869023.60 epoch sec; seqNo:3233; depth 50.45 2024-11-06T04:57:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869023.62 epoch sec; seqNo:3233; 4061641.39 north; 594876.59 east; 36.70 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T04:57:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869020.82 2024-11-06T04:57:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:03Z- waiting for 1730869024.82, time is 1730869023.62 2024-11-06T04:57:04Z- AHRS_M2 msg: 1730869024.00 epoch sec; seqNo:3032 2024-11-06T04:57:04Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.38 psi 2024-11-06T04:57:04Z- handleDepth()- Depth_Keller msg: 1730869024.01 epoch sec; seqNo:3234; depth 50.44 2024-11-06T04:57:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869024.03 epoch sec; seqNo:3234; 4061641.02 north; 594876.59 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T04:57:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869020.82 2024-11-06T04:57:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:04Z- waiting for 1730869024.82, time is 1730869024.03 2024-11-06T04:57:04Z- AHRS_M2 msg: 1730869024.41 epoch sec; seqNo:3033 2024-11-06T04:57:04Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.38 psi 2024-11-06T04:57:04Z- handleDepth()- Depth_Keller msg: 1730869024.42 epoch sec; seqNo:3235; depth 50.43 2024-11-06T04:57:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869024.43 epoch sec; seqNo:3235; 4061641.02 north; 594876.59 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:57:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869020.82 2024-11-06T04:57:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:04Z- waiting for 1730869024.82, time is 1730869024.43 2024-11-06T04:57:04Z- AHRS_M2 msg: 1730869024.82 epoch sec; seqNo:3034 2024-11-06T04:57:04Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.39 psi 2024-11-06T04:57:04Z- handleDepth()- Depth_Keller msg: 1730869024.82 epoch sec; seqNo:3236; depth 50.42 2024-11-06T04:57:04Z- handleDVL() - DVL msg epochMillisec: 1730869024.85 2024-11-06T04:57:04Z- handleDvl() - RDI_Pathfinder msg: 1730869024.85 sec; seqNo:1162; #beams:4 2024-11-06T04:57:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.34 , 37.92 , 43.42 , 44.03 2024-11-06T04:57:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.08 , 0.01 2024-11-06T04:57:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869024.87 epoch sec; seqNo:3236; 4061640.64 north; 594876.59 east; 36.70 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T04:57:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869020.82 2024-11-06T04:57:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:04Z- waiting for 1730869024.82, time is 1730869024.87 2024-11-06T04:57:04Z- LcmTrn::updateTrn() >>>> pose time 1730869024.87, meas time 1730869024.85 <<<< 2024-11-06T04:57:04Z- LcmTrn::updateTrn() equating pose 1730869024.85 and meas times 1730869024.85 2024-11-06T04:57:04Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869024.85 2024-11-06T04:57:04Z- TerrainNav::motionUpdate() - currEstimate = (4061641, 594876.59, 50.42) 2024-11-06T04:57:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:57:04Z- TerrainNav:: SubcloudNIS is 0.32, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:57:05Z- TNavPF::Projecting Measurements 2024-11-06T04:57:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:57:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:57:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:57:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:57:05Z- TNavFilter::4 good beams. 2024-11-06T04:57:05Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:57:05Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:57:05Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:57:05Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:57:05Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:57:05Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:57:05Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:57:05Z- TerrainMapOctree: (vehN, vehE ) = (4061640.64, 594876.59). 2024-11-06T04:57:05Z- TerrainMapOctree: Min Distance = 167.29. 2024-11-06T04:57:05Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:57:05Z- Forcing Subcloud Comparison 2024-11-06T04:57:05Z- Weighting particles with subcloud comparison 2024-11-06T04:57:05Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:05Z- meanExpectedMeasDiff: 1.472630 delta_rms_squared: 1.081693 alpha: 0.981846 2024-11-06T04:57:05Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:05Z- meanExpectedMeasDiff: -1.471688 delta_rms_squared: 0.769785 alpha: 0.974677 2024-11-06T04:57:05Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:05Z- meanExpectedMeasDiff: -0.673819 delta_rms_squared: 0.916635 alpha: 0.978647 2024-11-06T04:57:05Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:05Z- meanExpectedMeasDiff: 0.418331 delta_rms_squared: 0.896566 alpha: 0.978179 2024-11-06T04:57:05Z- Subcloud NIS: 0.317434 2024-11-06T04:57:05Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:57:05Z- TNavPF:: sumWeights = 0.452210 2024-11-06T04:57:05Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:57:05Z- TNavPF::First Term of Map Covariance Matrix is 0.60 2024-11-06T04:57:05Z- TNavPF::Calculated NIS Value : 0.28 numBeams normalized NIS: 0.07 2024-11-06T04:57:05Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869024.85 sec, ping # 0. 2024-11-06T04:57:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:57:05Z- variances([0], [2], and psi_berg) = 0.713720 0.292099 0.000000 2024-11-06T04:57:05Z- publishEstimatesDecoder() - at 1730869024.85 sec 2024-11-06T04:57:05Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:57:05Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061584.69 594881.07 59.20 2024-11-06T04:57:05Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061584.05 594881.01 59.14 lat/lon: 36.70 -121.94 2024-11-06T04:57:05Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.71 0.29 0.38 0.00 2024-11-06T04:57:05Z- AHRS_M2 msg: 1730869025.21 epoch sec; seqNo:3035 2024-11-06T04:57:05Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.39 psi 2024-11-06T04:57:05Z- handleDepth()- Depth_Keller msg: 1730869025.22 epoch sec; seqNo:3237; depth 50.41 2024-11-06T04:57:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869025.27 epoch sec; seqNo:3237; 4061640.26 north; 594876.60 east; 36.70 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T04:57:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869024.85 2024-11-06T04:57:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:05Z- waiting for 1730869028.85, time is 1730869025.27 2024-11-06T04:57:05Z- AHRS_M2 msg: 1730869025.62 epoch sec; seqNo:3036 2024-11-06T04:57:05Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.39 psi 2024-11-06T04:57:05Z- handleDepth()- Depth_Keller msg: 1730869025.62 epoch sec; seqNo:3238; depth 50.39 2024-11-06T04:57:05Z- handleDVL() - DVL msg epochMillisec: 1730869025.62 2024-11-06T04:57:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:57:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:57:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:57:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:57:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:57:05Z- handleDvl() - RDI_Pathfinder msg: 1730869025.62 sec; seqNo:1163; #beams:0 2024-11-06T04:57:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:57:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.08 , 0.01 2024-11-06T04:57:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869025.63 epoch sec; seqNo:3238; 4061639.88 north; 594876.60 east; 36.70 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:57:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869024.85 2024-11-06T04:57:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:05Z- waiting for 1730869028.85, time is 1730869025.63 2024-11-06T04:57:06Z- AHRS_M2 msg: 1730869026.02 epoch sec; seqNo:3037 2024-11-06T04:57:06Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.39 psi 2024-11-06T04:57:06Z- handleDepth()- Depth_Keller msg: 1730869026.03 epoch sec; seqNo:3239; depth 50.37 2024-11-06T04:57:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869026.03 epoch sec; seqNo:3239; 4061639.50 north; 594876.61 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:57:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869024.85 2024-11-06T04:57:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:06Z- waiting for 1730869028.85, time is 1730869026.03 2024-11-06T04:57:06Z- AHRS_M2 msg: 1730869026.42 epoch sec; seqNo:3038 2024-11-06T04:57:06Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.39 psi 2024-11-06T04:57:06Z- handleDepth()- Depth_Keller msg: 1730869026.43 epoch sec; seqNo:3240; depth 50.36 2024-11-06T04:57:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869026.44 epoch sec; seqNo:3240; 4061639.12 north; 594876.61 east; 36.70 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:57:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869024.85 2024-11-06T04:57:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:06Z- waiting for 1730869028.85, time is 1730869026.44 2024-11-06T04:57:06Z- AHRS_M2 msg: 1730869026.83 epoch sec; seqNo:3039 2024-11-06T04:57:06Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.39 psi 2024-11-06T04:57:06Z- handleDepth()- Depth_Keller msg: 1730869026.84 epoch sec; seqNo:3241; depth 50.33 2024-11-06T04:57:06Z- handleDVL() - DVL msg epochMillisec: 1730869026.86 2024-11-06T04:57:06Z- handleDvl() - RDI_Pathfinder msg: 1730869026.86 sec; seqNo:1164; #beams:4 2024-11-06T04:57:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.75 , 38.13 , 43.63 , 43.84 2024-11-06T04:57:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , -0.05 2024-11-06T04:57:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869026.88 epoch sec; seqNo:3241; 4061638.74 north; 594876.61 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T04:57:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869024.85 2024-11-06T04:57:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:06Z- waiting for 1730869028.85, time is 1730869026.88 2024-11-06T04:57:07Z- AHRS_M2 msg: 1730869027.23 epoch sec; seqNo:3040 2024-11-06T04:57:07Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.39 psi 2024-11-06T04:57:07Z- handleDepth()- Depth_Keller msg: 1730869027.24 epoch sec; seqNo:3242; depth 50.30 2024-11-06T04:57:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869027.25 epoch sec; seqNo:3242; 4061638.36 north; 594876.62 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:57:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869024.85 2024-11-06T04:57:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:07Z- waiting for 1730869028.85, time is 1730869027.25 2024-11-06T04:57:07Z- AHRS_M2 msg: 1730869027.63 epoch sec; seqNo:3041 2024-11-06T04:57:07Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.39 psi 2024-11-06T04:57:07Z- handleDepth()- Depth_Keller msg: 1730869027.65 epoch sec; seqNo:3243; depth 50.27 2024-11-06T04:57:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869027.66 epoch sec; seqNo:3243; 4061637.98 north; 594876.62 east; 36.70 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T04:57:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869024.85 2024-11-06T04:57:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:07Z- waiting for 1730869028.85, time is 1730869027.66 2024-11-06T04:57:08Z- AHRS_M2 msg: 1730869028.04 epoch sec; seqNo:3042 2024-11-06T04:57:08Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.39 psi 2024-11-06T04:57:08Z- handleDepth()- Depth_Keller msg: 1730869028.05 epoch sec; seqNo:3244; depth 50.25 2024-11-06T04:57:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869028.08 epoch sec; seqNo:3244; 4061637.61 north; 594876.63 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T04:57:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869024.85 2024-11-06T04:57:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:08Z- waiting for 1730869028.85, time is 1730869028.08 2024-11-06T04:57:08Z- AHRS_M2 msg: 1730869028.45 epoch sec; seqNo:3043 2024-11-06T04:57:08Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.39 psi 2024-11-06T04:57:08Z- handleDepth()- Depth_Keller msg: 1730869028.46 epoch sec; seqNo:3245; depth 50.23 2024-11-06T04:57:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869028.48 epoch sec; seqNo:3245; 4061637.21 north; 594875.41 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T04:57:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869024.85 2024-11-06T04:57:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:08Z- waiting for 1730869028.85, time is 1730869028.48 2024-11-06T04:57:08Z- AHRS_M2 msg: 1730869028.85 epoch sec; seqNo:3044 2024-11-06T04:57:08Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.39 psi 2024-11-06T04:57:08Z- handleDepth()- Depth_Keller msg: 1730869028.86 epoch sec; seqNo:3246; depth 50.20 2024-11-06T04:57:08Z- handleDVL() - DVL msg epochMillisec: 1730869028.88 2024-11-06T04:57:08Z- handleDvl() - RDI_Pathfinder msg: 1730869028.88 sec; seqNo:1165; #beams:4 2024-11-06T04:57:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.95 , 37.72 , 43.63 , 43.84 2024-11-06T04:57:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.08 , -0.05 2024-11-06T04:57:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869028.89 epoch sec; seqNo:3246; 4061636.83 north; 594875.42 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:57:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869024.85 2024-11-06T04:57:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:09Z- waiting for 1730869028.85, time is 1730869028.89 2024-11-06T04:57:09Z- LcmTrn::updateTrn() >>>> pose time 1730869028.89, meas time 1730869028.88 <<<< 2024-11-06T04:57:09Z- LcmTrn::updateTrn() equating pose 1730869028.88 and meas times 1730869028.88 2024-11-06T04:57:09Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869028.88 2024-11-06T04:57:09Z- TerrainNav::motionUpdate() - currEstimate = (4061637, 594875.42, 50.20) 2024-11-06T04:57:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:57:09Z- TerrainNav:: SubcloudNIS is 0.32, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:57:09Z- TNavPF::Projecting Measurements 2024-11-06T04:57:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:57:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:57:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:57:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:57:09Z- TNavFilter::4 good beams. 2024-11-06T04:57:09Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:57:09Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:57:09Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:57:09Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:57:09Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:57:09Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:57:09Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:57:09Z- TerrainMapOctree: (vehN, vehE ) = (4061636.83, 594875.42). 2024-11-06T04:57:09Z- TerrainMapOctree: Min Distance = 168.92. 2024-11-06T04:57:09Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:57:09Z- Forcing Subcloud Comparison 2024-11-06T04:57:09Z- Weighting particles with subcloud comparison 2024-11-06T04:57:09Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:09Z- meanExpectedMeasDiff: 1.570113 delta_rms_squared: 1.035326 alpha: 0.981049 2024-11-06T04:57:09Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:09Z- meanExpectedMeasDiff: -1.722388 delta_rms_squared: 0.722106 alpha: 0.973050 2024-11-06T04:57:09Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:09Z- meanExpectedMeasDiff: -0.774931 delta_rms_squared: 0.870618 alpha: 0.977544 2024-11-06T04:57:09Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:09Z- meanExpectedMeasDiff: 0.038940 delta_rms_squared: 0.846596 alpha: 0.976921 2024-11-06T04:57:09Z- Subcloud NIS: 0.305815 2024-11-06T04:57:09Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:57:09Z- TNavPF:: sumWeights = 0.411801 2024-11-06T04:57:09Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:57:09Z- TNavPF::First Term of Map Covariance Matrix is 0.67 2024-11-06T04:57:09Z- TNavPF::Calculated NIS Value : 0.26 numBeams normalized NIS: 0.07 2024-11-06T04:57:09Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869028.88 sec, ping # 0. 2024-11-06T04:57:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:57:09Z- variances([0], [2], and psi_berg) = 0.718074 0.296245 0.000000 2024-11-06T04:57:09Z- publishEstimatesDecoder() - at 1730869028.88 sec 2024-11-06T04:57:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:57:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061579.72 594880.06 59.19 2024-11-06T04:57:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061580.24 594879.84 59.04 lat/lon: 36.70 -121.94 2024-11-06T04:57:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.72 0.30 0.37 0.00 2024-11-06T04:57:09Z- AHRS_M2 msg: 1730869029.25 epoch sec; seqNo:3045 2024-11-06T04:57:09Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.39 psi 2024-11-06T04:57:09Z- handleDepth()- Depth_Keller msg: 1730869029.26 epoch sec; seqNo:3247; depth 50.19 2024-11-06T04:57:09Z- handleDVL() - DVL msg epochMillisec: 1730869029.26 2024-11-06T04:57:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:57:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:57:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:57:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:57:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:57:09Z- handleDvl() - RDI_Pathfinder msg: 1730869029.26 sec; seqNo:1166; #beams:0 2024-11-06T04:57:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:57:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.08 , -0.05 2024-11-06T04:57:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869029.29 epoch sec; seqNo:3247; 4061636.46 north; 594875.42 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:57:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869028.88 2024-11-06T04:57:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:09Z- waiting for 1730869032.88, time is 1730869029.29 2024-11-06T04:57:09Z- AHRS_M2 msg: 1730869029.65 epoch sec; seqNo:3046 2024-11-06T04:57:09Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.39 psi 2024-11-06T04:57:09Z- handleDepth()- Depth_Keller msg: 1730869029.67 epoch sec; seqNo:3248; depth 50.15 2024-11-06T04:57:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869029.68 epoch sec; seqNo:3248; 4061636.08 north; 594875.42 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T04:57:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869028.88 2024-11-06T04:57:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:09Z- waiting for 1730869032.88, time is 1730869029.68 2024-11-06T04:57:10Z- AHRS_M2 msg: 1730869030.06 epoch sec; seqNo:3047 2024-11-06T04:57:10Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 3.39 psi 2024-11-06T04:57:10Z- handleDepth()- Depth_Keller msg: 1730869030.07 epoch sec; seqNo:3249; depth 50.14 2024-11-06T04:57:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869030.08 epoch sec; seqNo:3249; 4061635.70 north; 594875.43 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:57:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869028.88 2024-11-06T04:57:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:10Z- waiting for 1730869032.88, time is 1730869030.08 2024-11-06T04:57:10Z- AHRS_M2 msg: 1730869030.47 epoch sec; seqNo:3048 2024-11-06T04:57:10Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 3.38 psi 2024-11-06T04:57:10Z- handleDepth()- Depth_Keller msg: 1730869030.48 epoch sec; seqNo:3250; depth 50.10 2024-11-06T04:57:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869030.49 epoch sec; seqNo:3250; 4061635.70 north; 594875.43 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T04:57:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869028.88 2024-11-06T04:57:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:10Z- waiting for 1730869032.88, time is 1730869030.49 2024-11-06T04:57:10Z- AHRS_M2 msg: 1730869030.87 epoch sec; seqNo:3049 2024-11-06T04:57:10Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 3.38 psi 2024-11-06T04:57:10Z- handleDepth()- Depth_Keller msg: 1730869030.88 epoch sec; seqNo:3251; depth 50.11 2024-11-06T04:57:10Z- handleDVL() - DVL msg epochMillisec: 1730869030.91 2024-11-06T04:57:10Z- handleDvl() - RDI_Pathfinder msg: 1730869030.91 sec; seqNo:1167; #beams:4 2024-11-06T04:57:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.74 , 37.72 , 43.42 , 43.83 2024-11-06T04:57:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.16 , 0.02 2024-11-06T04:57:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869030.92 epoch sec; seqNo:3251; 4061635.32 north; 594875.43 east; 36.70 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T04:57:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869028.88 2024-11-06T04:57:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:11Z- waiting for 1730869032.88, time is 1730869030.92 2024-11-06T04:57:11Z- AHRS_M2 msg: 1730869031.27 epoch sec; seqNo:3050 2024-11-06T04:57:11Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.38 psi 2024-11-06T04:57:11Z- handleDepth()- Depth_Keller msg: 1730869031.28 epoch sec; seqNo:3252; depth 50.09 2024-11-06T04:57:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869031.29 epoch sec; seqNo:3252; 4061634.94 north; 594875.44 east; 36.70 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T04:57:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869028.88 2024-11-06T04:57:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:11Z- waiting for 1730869032.88, time is 1730869031.29 2024-11-06T04:57:11Z- AHRS_M2 msg: 1730869031.67 epoch sec; seqNo:3051 2024-11-06T04:57:11Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.38 psi 2024-11-06T04:57:11Z- handleDepth()- Depth_Keller msg: 1730869031.68 epoch sec; seqNo:3253; depth 50.07 2024-11-06T04:57:11Z- handleDVL() - DVL msg epochMillisec: 1730869031.69 2024-11-06T04:57:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:57:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:57:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:57:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:57:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:57:11Z- handleDvl() - RDI_Pathfinder msg: 1730869031.69 sec; seqNo:1168; #beams:0 2024-11-06T04:57:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:57:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.16 , 0.02 2024-11-06T04:57:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869031.70 epoch sec; seqNo:3253; 4061634.56 north; 594875.44 east; 36.70 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T04:57:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869028.88 2024-11-06T04:57:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:11Z- waiting for 1730869032.88, time is 1730869031.70 2024-11-06T04:57:12Z- AHRS_M2 msg: 1730869032.08 epoch sec; seqNo:3052 2024-11-06T04:57:12Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.38 psi 2024-11-06T04:57:12Z- handleDepth()- Depth_Keller msg: 1730869032.08 epoch sec; seqNo:3254; depth 50.05 2024-11-06T04:57:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869032.09 epoch sec; seqNo:3254; 4061634.17 north; 594874.22 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T04:57:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869028.88 2024-11-06T04:57:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:12Z- waiting for 1730869032.88, time is 1730869032.09 2024-11-06T04:57:12Z- AHRS_M2 msg: 1730869032.49 epoch sec; seqNo:3053 2024-11-06T04:57:12Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.37 psi 2024-11-06T04:57:12Z- handleDepth()- Depth_Keller msg: 1730869032.49 epoch sec; seqNo:3255; depth 50.05 2024-11-06T04:57:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869032.51 epoch sec; seqNo:3255; 4061633.79 north; 594874.23 east; 36.70 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T04:57:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869028.88 2024-11-06T04:57:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:12Z- waiting for 1730869032.88, time is 1730869032.51 2024-11-06T04:57:12Z- AHRS_M2 msg: 1730869032.89 epoch sec; seqNo:3054 2024-11-06T04:57:12Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.37 psi 2024-11-06T04:57:12Z- handleDepth()- Depth_Keller msg: 1730869032.90 epoch sec; seqNo:3256; depth 50.03 2024-11-06T04:57:12Z- handleDVL() - DVL msg epochMillisec: 1730869032.92 2024-11-06T04:57:12Z- handleDvl() - RDI_Pathfinder msg: 1730869032.92 sec; seqNo:1169; #beams:4 2024-11-06T04:57:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.75 , 37.72 , 43.02 , 43.84 2024-11-06T04:57:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.12 , 0.08 2024-11-06T04:57:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869032.95 epoch sec; seqNo:3256; 4061633.41 north; 594874.23 east; 36.70 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T04:57:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869028.88 2024-11-06T04:57:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:13Z- waiting for 1730869032.88, time is 1730869032.95 2024-11-06T04:57:13Z- LcmTrn::updateTrn() >>>> pose time 1730869032.95, meas time 1730869032.92 <<<< 2024-11-06T04:57:13Z- LcmTrn::updateTrn() equating pose 1730869032.92 and meas times 1730869032.92 2024-11-06T04:57:13Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869032.92 2024-11-06T04:57:13Z- TerrainNav::motionUpdate() - currEstimate = (4061633, 594874.23, 50.03) 2024-11-06T04:57:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:57:13Z- TerrainNav:: SubcloudNIS is 0.31, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:57:13Z- TNavPF::Projecting Measurements 2024-11-06T04:57:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:57:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:57:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:57:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:57:13Z- TNavFilter::4 good beams. 2024-11-06T04:57:13Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:57:13Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:57:13Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:57:13Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:57:13Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:57:13Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:57:13Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:57:13Z- TerrainMapOctree: (vehN, vehE ) = (4061633.41, 594874.23). 2024-11-06T04:57:13Z- TerrainMapOctree: Min Distance = 170.37. 2024-11-06T04:57:13Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:57:13Z- Forcing Subcloud Comparison 2024-11-06T04:57:13Z- Weighting particles with subcloud comparison 2024-11-06T04:57:13Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:13Z- meanExpectedMeasDiff: 1.416365 delta_rms_squared: 1.014893 alpha: 0.980674 2024-11-06T04:57:13Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:13Z- meanExpectedMeasDiff: -1.852775 delta_rms_squared: 0.714876 alpha: 0.972785 2024-11-06T04:57:13Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:13Z- meanExpectedMeasDiff: -1.430819 delta_rms_squared: 0.856912 alpha: 0.977193 2024-11-06T04:57:13Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:13Z- meanExpectedMeasDiff: -0.031482 delta_rms_squared: 0.834308 alpha: 0.976589 2024-11-06T04:57:13Z- Subcloud NIS: 0.318413 2024-11-06T04:57:13Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:57:13Z- TNavPF:: sumWeights = 0.361137 2024-11-06T04:57:13Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:57:13Z- TNavPF::First Term of Map Covariance Matrix is 0.63 2024-11-06T04:57:13Z- TNavPF::Calculated NIS Value : 0.23 numBeams normalized NIS: 0.06 2024-11-06T04:57:13Z- TerrainNav::Resampling particle filter... 2024-11-06T04:57:13Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869032.92 sec, ping # 0. 2024-11-06T04:57:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:57:13Z- variances([0], [2], and psi_berg) = 0.717995 0.299013 0.000000 2024-11-06T04:57:13Z- publishEstimatesDecoder() - at 1730869032.92 sec 2024-11-06T04:57:13Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:57:13Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061578.76 594878.71 58.76 2024-11-06T04:57:13Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061576.81 594878.66 59.05 lat/lon: 36.70 -121.94 2024-11-06T04:57:13Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.72 0.30 0.37 0.00 2024-11-06T04:57:13Z- AHRS_M2 msg: 1730869033.29 epoch sec; seqNo:3055 2024-11-06T04:57:13Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.37 psi 2024-11-06T04:57:13Z- handleDepth()- Depth_Keller msg: 1730869033.30 epoch sec; seqNo:3257; depth 50.02 2024-11-06T04:57:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869033.32 epoch sec; seqNo:3257; 4061633.03 north; 594874.24 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:57:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869032.92 2024-11-06T04:57:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:13Z- waiting for 1730869036.92, time is 1730869033.32 2024-11-06T04:57:13Z- AHRS_M2 msg: 1730869033.69 epoch sec; seqNo:3056 2024-11-06T04:57:13Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.37 psi 2024-11-06T04:57:13Z- handleDepth()- Depth_Keller msg: 1730869033.70 epoch sec; seqNo:3258; depth 50.01 2024-11-06T04:57:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869033.71 epoch sec; seqNo:3258; 4061632.65 north; 594874.24 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:57:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869032.92 2024-11-06T04:57:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:13Z- waiting for 1730869036.92, time is 1730869033.71 2024-11-06T04:57:14Z- AHRS_M2 msg: 1730869034.10 epoch sec; seqNo:3057 2024-11-06T04:57:14Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.37 psi 2024-11-06T04:57:14Z- handleDepth()- Depth_Keller msg: 1730869034.10 epoch sec; seqNo:3259; depth 50.01 2024-11-06T04:57:14Z- handleDVL() - DVL msg epochMillisec: 1730869034.11 2024-11-06T04:57:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:57:14Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:57:14Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:57:14Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:57:14Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:57:14Z- handleDvl() - RDI_Pathfinder msg: 1730869034.11 sec; seqNo:1170; #beams:0 2024-11-06T04:57:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:57:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.12 , 0.08 2024-11-06T04:57:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869034.12 epoch sec; seqNo:3259; 4061632.27 north; 594874.25 east; 36.70 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:57:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869032.92 2024-11-06T04:57:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:14Z- waiting for 1730869036.92, time is 1730869034.12 2024-11-06T04:57:14Z- AHRS_M2 msg: 1730869034.51 epoch sec; seqNo:3058 2024-11-06T04:57:14Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.37 psi 2024-11-06T04:57:14Z- handleDepth()- Depth_Keller msg: 1730869034.52 epoch sec; seqNo:3260; depth 49.99 2024-11-06T04:57:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869034.53 epoch sec; seqNo:3260; 4061631.90 north; 594874.25 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T04:57:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869032.92 2024-11-06T04:57:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:14Z- waiting for 1730869036.92, time is 1730869034.53 2024-11-06T04:57:14Z- AHRS_M2 msg: 1730869034.91 epoch sec; seqNo:3059 2024-11-06T04:57:14Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.37 psi 2024-11-06T04:57:14Z- handleDepth()- Depth_Keller msg: 1730869034.91 epoch sec; seqNo:3261; depth 49.99 2024-11-06T04:57:14Z- handleDVL() - DVL msg epochMillisec: 1730869034.94 2024-11-06T04:57:14Z- handleDvl() - RDI_Pathfinder msg: 1730869034.94 sec; seqNo:1171; #beams:4 2024-11-06T04:57:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.54 , 37.92 , 42.81 , 43.63 2024-11-06T04:57:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , 0.09 2024-11-06T04:57:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869034.95 epoch sec; seqNo:3261; 4061631.52 north; 594874.25 east; 36.70 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T04:57:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869032.92 2024-11-06T04:57:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:15Z- waiting for 1730869036.92, time is 1730869034.95 2024-11-06T04:57:15Z- AHRS_M2 msg: 1730869035.31 epoch sec; seqNo:3060 2024-11-06T04:57:15Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.37 psi 2024-11-06T04:57:15Z- handleDepth()- Depth_Keller msg: 1730869035.32 epoch sec; seqNo:3262; depth 49.98 2024-11-06T04:57:15Z- handleDVL() - DVL msg epochMillisec: 1730869035.32 2024-11-06T04:57:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:57:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:57:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:57:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:57:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:57:15Z- handleDvl() - RDI_Pathfinder msg: 1730869035.32 sec; seqNo:1172; #beams:0 2024-11-06T04:57:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:57:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , 0.09 2024-11-06T04:57:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869035.33 epoch sec; seqNo:3262; 4061631.52 north; 594874.25 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T04:57:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869032.92 2024-11-06T04:57:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:15Z- waiting for 1730869036.92, time is 1730869035.33 2024-11-06T04:57:15Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:57:15Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:57:15Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:57:15Z- AHRS_M2 msg: 1730869035.71 epoch sec; seqNo:3061 2024-11-06T04:57:15Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.37 psi 2024-11-06T04:57:15Z- handleDepth()- Depth_Keller msg: 1730869035.74 epoch sec; seqNo:3263; depth 49.98 2024-11-06T04:57:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869035.75 epoch sec; seqNo:3263; 4061631.14 north; 594874.26 east; 36.70 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T04:57:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869032.92 2024-11-06T04:57:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:15Z- waiting for 1730869036.92, time is 1730869035.75 2024-11-06T04:57:16Z- AHRS_M2 msg: 1730869036.12 epoch sec; seqNo:3062 2024-11-06T04:57:16Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.37 psi 2024-11-06T04:57:16Z- handleDepth()- Depth_Keller msg: 1730869036.12 epoch sec; seqNo:3264; depth 49.97 2024-11-06T04:57:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869036.17 epoch sec; seqNo:3264; 4061630.76 north; 594874.26 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T04:57:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869032.92 2024-11-06T04:57:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:16Z- waiting for 1730869036.92, time is 1730869036.17 2024-11-06T04:57:16Z- handleDepth()- Depth_Keller msg: 1730869036.56 epoch sec; seqNo:3265; depth 49.96 2024-11-06T04:57:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869036.57 epoch sec; seqNo:3265; 4061630.37 north; 594873.05 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:57:16Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:57:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869032.92 2024-11-06T04:57:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:16Z- waiting for 1730869036.92, time is 1730869036.57 2024-11-06T04:57:16Z- AHRS_M2 msg: 1730869036.93 epoch sec; seqNo:3063 2024-11-06T04:57:16Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 3.38 psi 2024-11-06T04:57:16Z- handleDepth()- Depth_Keller msg: 1730869036.93 epoch sec; seqNo:3266; depth 49.96 2024-11-06T04:57:16Z- handleDVL() - DVL msg epochMillisec: 1730869036.95 2024-11-06T04:57:16Z- handleDvl() - RDI_Pathfinder msg: 1730869036.95 sec; seqNo:1173; #beams:4 2024-11-06T04:57:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.54 , 38.13 , 42.41 , 43.84 2024-11-06T04:57:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , 0.05 2024-11-06T04:57:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869036.98 epoch sec; seqNo:3266; 4061629.99 north; 594873.05 east; 36.70 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:57:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869032.92 2024-11-06T04:57:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:17Z- waiting for 1730869036.92, time is 1730869036.98 2024-11-06T04:57:17Z- LcmTrn::updateTrn() >>>> pose time 1730869036.98, meas time 1730869036.95 <<<< 2024-11-06T04:57:17Z- LcmTrn::updateTrn() equating pose 1730869036.95 and meas times 1730869036.95 2024-11-06T04:57:17Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869036.95 2024-11-06T04:57:17Z- TerrainNav::motionUpdate() - currEstimate = (4061630, 594873.05, 49.96) 2024-11-06T04:57:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:57:17Z- TerrainNav:: SubcloudNIS is 0.32, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:57:17Z- TNavPF::Projecting Measurements 2024-11-06T04:57:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:57:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:57:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:57:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:57:17Z- TNavFilter::4 good beams. 2024-11-06T04:57:17Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:57:17Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:57:17Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:57:17Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:57:17Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:57:17Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:57:17Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:57:17Z- TerrainMapOctree: (vehN, vehE ) = (4061629.99, 594873.05). 2024-11-06T04:57:17Z- TerrainMapOctree: Min Distance = 171.88. 2024-11-06T04:57:17Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:57:17Z- Forcing Subcloud Comparison 2024-11-06T04:57:17Z- Weighting particles with subcloud comparison 2024-11-06T04:57:17Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:17Z- meanExpectedMeasDiff: 1.093052 delta_rms_squared: 1.006465 alpha: 0.980516 2024-11-06T04:57:17Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:17Z- meanExpectedMeasDiff: -1.227117 delta_rms_squared: 0.697301 alpha: 0.972118 2024-11-06T04:57:17Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:17Z- meanExpectedMeasDiff: -1.885463 delta_rms_squared: 0.839508 alpha: 0.976731 2024-11-06T04:57:17Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:17Z- meanExpectedMeasDiff: -0.024939 delta_rms_squared: 0.822998 alpha: 0.976275 2024-11-06T04:57:17Z- Subcloud NIS: 0.327367 2024-11-06T04:57:17Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:57:17Z- TNavPF:: sumWeights = 0.417750 2024-11-06T04:57:17Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:57:17Z- TNavPF::First Term of Map Covariance Matrix is 0.51 2024-11-06T04:57:17Z- TNavPF::Calculated NIS Value : 0.25 numBeams normalized NIS: 0.06 2024-11-06T04:57:17Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869036.95 sec, ping # 0. 2024-11-06T04:57:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:57:17Z- variances([0], [2], and psi_berg) = 0.719641 0.300692 0.000000 2024-11-06T04:57:17Z- publishEstimatesDecoder() - at 1730869036.95 sec 2024-11-06T04:57:17Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:57:17Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061572.54 594878.14 59.45 2024-11-06T04:57:17Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061573.39 594877.49 59.17 lat/lon: 36.70 -121.94 2024-11-06T04:57:17Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.72 0.30 0.37 0.00 2024-11-06T04:57:17Z- AHRS_M2 msg: 1730869037.33 epoch sec; seqNo:3064 2024-11-06T04:57:17Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 3.38 psi 2024-11-06T04:57:17Z- handleDepth()- Depth_Keller msg: 1730869037.35 epoch sec; seqNo:3267; depth 49.93 2024-11-06T04:57:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869037.37 epoch sec; seqNo:3267; 4061629.61 north; 594873.05 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T04:57:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869036.95 2024-11-06T04:57:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:17Z- waiting for 1730869040.95, time is 1730869037.37 2024-11-06T04:57:17Z- AHRS_M2 msg: 1730869037.73 epoch sec; seqNo:3065 2024-11-06T04:57:17Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 3.38 psi 2024-11-06T04:57:17Z- handleDepth()- Depth_Keller msg: 1730869037.74 epoch sec; seqNo:3268; depth 49.92 2024-11-06T04:57:17Z- handleDVL() - DVL msg epochMillisec: 1730869037.74 2024-11-06T04:57:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:57:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:57:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:57:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:57:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:57:17Z- handleDvl() - RDI_Pathfinder msg: 1730869037.74 sec; seqNo:1174; #beams:0 2024-11-06T04:57:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:57:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.02 , 0.05 2024-11-06T04:57:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869037.77 epoch sec; seqNo:3268; 4061629.23 north; 594873.06 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:57:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869036.95 2024-11-06T04:57:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:17Z- waiting for 1730869040.95, time is 1730869037.77 2024-11-06T04:57:18Z- AHRS_M2 msg: 1730869038.14 epoch sec; seqNo:3066 2024-11-06T04:57:18Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.38 psi 2024-11-06T04:57:18Z- handleDepth()- Depth_Keller msg: 1730869038.15 epoch sec; seqNo:3269; depth 49.93 2024-11-06T04:57:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869038.16 epoch sec; seqNo:3269; 4061628.85 north; 594873.06 east; 36.70 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T04:57:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869036.95 2024-11-06T04:57:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:18Z- waiting for 1730869040.95, time is 1730869038.16 2024-11-06T04:57:18Z- AHRS_M2 msg: 1730869038.55 epoch sec; seqNo:3067 2024-11-06T04:57:18Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.38 psi 2024-11-06T04:57:18Z- handleDepth()- Depth_Keller msg: 1730869038.56 epoch sec; seqNo:3270; depth 49.90 2024-11-06T04:57:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869038.56 epoch sec; seqNo:3270; 4061628.47 north; 594873.07 east; 36.70 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:57:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869036.95 2024-11-06T04:57:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:18Z- waiting for 1730869040.95, time is 1730869038.56 2024-11-06T04:57:18Z- AHRS_M2 msg: 1730869038.95 epoch sec; seqNo:3068 2024-11-06T04:57:18Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.38 psi 2024-11-06T04:57:18Z- handleDepth()- Depth_Keller msg: 1730869038.96 epoch sec; seqNo:3271; depth 49.89 2024-11-06T04:57:18Z- handleDVL() - DVL msg epochMillisec: 1730869038.98 2024-11-06T04:57:18Z- handleDvl() - RDI_Pathfinder msg: 1730869038.98 sec; seqNo:1175; #beams:4 2024-11-06T04:57:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.35 , 38.12 , 42.81 , 44.03 2024-11-06T04:57:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.00 2024-11-06T04:57:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869038.99 epoch sec; seqNo:3271; 4061628.09 north; 594873.07 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:57:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869036.95 2024-11-06T04:57:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:19Z- waiting for 1730869040.95, time is 1730869038.99 2024-11-06T04:57:19Z- AHRS_M2 msg: 1730869039.35 epoch sec; seqNo:3069 2024-11-06T04:57:19Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.38 psi 2024-11-06T04:57:19Z- handleDepth()- Depth_Keller msg: 1730869039.36 epoch sec; seqNo:3272; depth 49.86 2024-11-06T04:57:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869039.38 epoch sec; seqNo:3272; 4061627.72 north; 594873.08 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:57:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869036.95 2024-11-06T04:57:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:19Z- waiting for 1730869040.95, time is 1730869039.38 2024-11-06T04:57:19Z- AHRS_M2 msg: 1730869039.76 epoch sec; seqNo:3070 2024-11-06T04:57:19Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.38 psi 2024-11-06T04:57:19Z- handleDepth()- Depth_Keller msg: 1730869039.77 epoch sec; seqNo:3273; depth 49.85 2024-11-06T04:57:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869039.78 epoch sec; seqNo:3273; 4061627.34 north; 594873.08 east; 36.70 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:57:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869036.95 2024-11-06T04:57:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:19Z- waiting for 1730869040.95, time is 1730869039.78 2024-11-06T04:57:20Z- AHRS_M2 msg: 1730869040.16 epoch sec; seqNo:3071 2024-11-06T04:57:20Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.38 psi 2024-11-06T04:57:20Z- handleDepth()- Depth_Keller msg: 1730869040.16 epoch sec; seqNo:3274; depth 49.84 2024-11-06T04:57:20Z- handleDVL() - DVL msg epochMillisec: 1730869040.17 2024-11-06T04:57:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:57:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:57:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:57:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:57:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:57:20Z- handleDvl() - RDI_Pathfinder msg: 1730869040.17 sec; seqNo:1176; #beams:0 2024-11-06T04:57:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:57:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.00 2024-11-06T04:57:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869040.17 epoch sec; seqNo:3274; 4061626.96 north; 594873.08 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T04:57:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869036.95 2024-11-06T04:57:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:20Z- waiting for 1730869040.95, time is 1730869040.17 2024-11-06T04:57:20Z- AHRS_M2 msg: 1730869040.56 epoch sec; seqNo:3072 2024-11-06T04:57:20Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.38 psi 2024-11-06T04:57:20Z- handleDepth()- Depth_Keller msg: 1730869040.58 epoch sec; seqNo:3275; depth 49.81 2024-11-06T04:57:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869040.60 epoch sec; seqNo:3275; 4061626.58 north; 594873.09 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:57:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869036.95 2024-11-06T04:57:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:20Z- waiting for 1730869040.95, time is 1730869040.60 2024-11-06T04:57:20Z- AHRS_M2 msg: 1730869040.97 epoch sec; seqNo:3073 2024-11-06T04:57:20Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 3.38 psi 2024-11-06T04:57:20Z- handleDepth()- Depth_Keller msg: 1730869040.97 epoch sec; seqNo:3276; depth 49.80 2024-11-06T04:57:21Z- handleDVL() - DVL msg epochMillisec: 1730869040.99 2024-11-06T04:57:21Z- handleDvl() - RDI_Pathfinder msg: 1730869040.99 sec; seqNo:1177; #beams:4 2024-11-06T04:57:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.15 , 37.72 , 42.40 , 44.03 2024-11-06T04:57:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.01 , -0.03 2024-11-06T04:57:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869041.03 epoch sec; seqNo:3276; 4061626.20 north; 594873.09 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T04:57:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869036.95 2024-11-06T04:57:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:21Z- waiting for 1730869040.95, time is 1730869041.03 2024-11-06T04:57:21Z- LcmTrn::updateTrn() >>>> pose time 1730869041.03, meas time 1730869040.99 <<<< 2024-11-06T04:57:21Z- LcmTrn::updateTrn() equating pose 1730869040.99 and meas times 1730869040.99 2024-11-06T04:57:21Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869040.99 2024-11-06T04:57:21Z- TerrainNav::motionUpdate() - currEstimate = (4061626, 594873.09, 49.80) 2024-11-06T04:57:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:57:21Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:57:21Z- TNavPF::Projecting Measurements 2024-11-06T04:57:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:57:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:57:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:57:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:57:21Z- TNavFilter::4 good beams. 2024-11-06T04:57:21Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:57:21Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:57:21Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:57:21Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:57:21Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:57:21Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:57:21Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:57:21Z- TerrainMapOctree: (vehN, vehE ) = (4061626.20, 594873.09). 2024-11-06T04:57:21Z- TerrainMapOctree: Min Distance = 174.58. 2024-11-06T04:57:21Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:57:21Z- Forcing Subcloud Comparison 2024-11-06T04:57:21Z- Weighting particles with subcloud comparison 2024-11-06T04:57:21Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:21Z- meanExpectedMeasDiff: 1.705762 delta_rms_squared: 1.011819 alpha: 0.980617 2024-11-06T04:57:21Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:21Z- meanExpectedMeasDiff: -1.587153 delta_rms_squared: 0.698643 alpha: 0.972170 2024-11-06T04:57:21Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:21Z- meanExpectedMeasDiff: -1.590331 delta_rms_squared: 0.832685 alpha: 0.976545 2024-11-06T04:57:21Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:21Z- meanExpectedMeasDiff: -0.055354 delta_rms_squared: 0.836368 alpha: 0.976646 2024-11-06T04:57:21Z- Subcloud NIS: 0.328863 2024-11-06T04:57:21Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:57:21Z- TNavPF:: sumWeights = 0.349872 2024-11-06T04:57:21Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:57:21Z- TNavPF::First Term of Map Covariance Matrix is 0.75 2024-11-06T04:57:21Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.05 2024-11-06T04:57:21Z- TerrainNav::Resampling particle filter... 2024-11-06T04:57:21Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869040.99 sec, ping # 0. 2024-11-06T04:57:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:57:21Z- variances([0], [2], and psi_berg) = 0.727901 0.302893 0.000000 2024-11-06T04:57:21Z- publishEstimatesDecoder() - at 1730869040.99 sec 2024-11-06T04:57:21Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:57:21Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061571.64 594877.62 58.20 2024-11-06T04:57:21Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061569.60 594877.53 59.16 lat/lon: 36.70 -121.94 2024-11-06T04:57:21Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.73 0.30 0.37 0.00 2024-11-06T04:57:21Z- handleDepth()- Depth_Keller msg: 1730869041.39 epoch sec; seqNo:3277; depth 49.78 2024-11-06T04:57:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869041.41 epoch sec; seqNo:3277; 4061626.20 north; 594873.09 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:57:21Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:57:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869040.99 2024-11-06T04:57:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:21Z- waiting for 1730869044.99, time is 1730869041.41 2024-11-06T04:57:21Z- AHRS_M2 msg: 1730869041.78 epoch sec; seqNo:3074 2024-11-06T04:57:21Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.38 psi 2024-11-06T04:57:21Z- handleDepth()- Depth_Keller msg: 1730869041.79 epoch sec; seqNo:3278; depth 49.76 2024-11-06T04:57:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869041.80 epoch sec; seqNo:3278; 4061625.82 north; 594873.10 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T04:57:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869040.99 2024-11-06T04:57:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:21Z- waiting for 1730869044.99, time is 1730869041.80 2024-11-06T04:57:22Z- AHRS_M2 msg: 1730869042.18 epoch sec; seqNo:3075 2024-11-06T04:57:22Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.38 psi 2024-11-06T04:57:22Z- handleDepth()- Depth_Keller msg: 1730869042.18 epoch sec; seqNo:3279; depth 49.75 2024-11-06T04:57:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869042.19 epoch sec; seqNo:3279; 4061625.44 north; 594873.10 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:57:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869040.99 2024-11-06T04:57:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:22Z- waiting for 1730869044.99, time is 1730869042.19 2024-11-06T04:57:22Z- AHRS_M2 msg: 1730869042.59 epoch sec; seqNo:3076 2024-11-06T04:57:22Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.37 psi 2024-11-06T04:57:22Z- handleDepth()- Depth_Keller msg: 1730869042.60 epoch sec; seqNo:3280; depth 49.74 2024-11-06T04:57:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869042.61 epoch sec; seqNo:3280; 4061625.05 north; 594871.88 east; 36.70 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T04:57:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869040.99 2024-11-06T04:57:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:22Z- waiting for 1730869044.99, time is 1730869042.61 2024-11-06T04:57:22Z- AHRS_M2 msg: 1730869042.99 epoch sec; seqNo:3077 2024-11-06T04:57:22Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.37 psi 2024-11-06T04:57:22Z- handleDepth()- Depth_Keller msg: 1730869042.99 epoch sec; seqNo:3281; depth 49.71 2024-11-06T04:57:23Z- handleDVL() - DVL msg epochMillisec: 1730869043.01 2024-11-06T04:57:23Z- handleDvl() - RDI_Pathfinder msg: 1730869043.01 sec; seqNo:1178; #beams:4 2024-11-06T04:57:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.36 , 37.51 , 42.41 , 44.45 2024-11-06T04:57:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.09 , -0.01 2024-11-06T04:57:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869043.03 epoch sec; seqNo:3281; 4061624.67 north; 594871.89 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T04:57:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869040.99 2024-11-06T04:57:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:23Z- waiting for 1730869044.99, time is 1730869043.03 2024-11-06T04:57:23Z- handleDepth()- Depth_Keller msg: 1730869043.41 epoch sec; seqNo:3282; depth 49.72 2024-11-06T04:57:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869043.42 epoch sec; seqNo:3282; 4061624.29 north; 594871.89 east; 36.70 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:57:23Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:57:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869040.99 2024-11-06T04:57:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:23Z- waiting for 1730869044.99, time is 1730869043.42 2024-11-06T04:57:23Z- AHRS_M2 msg: 1730869043.79 epoch sec; seqNo:3078 2024-11-06T04:57:23Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.37 psi 2024-11-06T04:57:23Z- handleDepth()- Depth_Keller msg: 1730869043.82 epoch sec; seqNo:3283; depth 49.70 2024-11-06T04:57:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869043.83 epoch sec; seqNo:3283; 4061623.91 north; 594871.90 east; 36.70 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T04:57:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869040.99 2024-11-06T04:57:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:23Z- waiting for 1730869044.99, time is 1730869043.83 2024-11-06T04:57:24Z- AHRS_M2 msg: 1730869044.20 epoch sec; seqNo:3079 2024-11-06T04:57:24Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.36 psi 2024-11-06T04:57:24Z- handleDepth()- Depth_Keller msg: 1730869044.21 epoch sec; seqNo:3284; depth 49.68 2024-11-06T04:57:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869044.21 epoch sec; seqNo:3284; 4061623.53 north; 594871.90 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T04:57:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869040.99 2024-11-06T04:57:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:24Z- waiting for 1730869044.99, time is 1730869044.21 2024-11-06T04:57:24Z- AHRS_M2 msg: 1730869044.61 epoch sec; seqNo:3080 2024-11-06T04:57:24Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.36 psi 2024-11-06T04:57:24Z- handleDepth()- Depth_Keller msg: 1730869044.61 epoch sec; seqNo:3285; depth 49.69 2024-11-06T04:57:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869044.62 epoch sec; seqNo:3285; 4061623.16 north; 594871.91 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T04:57:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869040.99 2024-11-06T04:57:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:24Z- waiting for 1730869044.99, time is 1730869044.62 2024-11-06T04:57:25Z- AHRS_M2 msg: 1730869045.01 epoch sec; seqNo:3081 2024-11-06T04:57:25Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.36 psi 2024-11-06T04:57:25Z- handleDepth()- Depth_Keller msg: 1730869045.02 epoch sec; seqNo:3286; depth 49.67 2024-11-06T04:57:25Z- handleDVL() - DVL msg epochMillisec: 1730869045.04 2024-11-06T04:57:25Z- handleDvl() - RDI_Pathfinder msg: 1730869045.04 sec; seqNo:1179; #beams:4 2024-11-06T04:57:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.16 , 37.51 , 42.00 , 44.45 2024-11-06T04:57:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.11 , 0.05 2024-11-06T04:57:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869045.05 epoch sec; seqNo:3286; 4061622.78 north; 594871.91 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:57:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869040.99 2024-11-06T04:57:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:25Z- waiting for 1730869044.99, time is 1730869045.05 2024-11-06T04:57:25Z- LcmTrn::updateTrn() >>>> pose time 1730869045.05, meas time 1730869045.04 <<<< 2024-11-06T04:57:25Z- LcmTrn::updateTrn() equating pose 1730869045.04 and meas times 1730869045.04 2024-11-06T04:57:25Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869045.04 2024-11-06T04:57:25Z- TerrainNav::motionUpdate() - currEstimate = (4061623, 594871.91, 49.67) 2024-11-06T04:57:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:57:25Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:57:25Z- TNavPF::Projecting Measurements 2024-11-06T04:57:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:57:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:57:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:57:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:57:25Z- TNavFilter::4 good beams. 2024-11-06T04:57:25Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:57:25Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:57:25Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:57:25Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:57:25Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:57:25Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:57:25Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:57:25Z- TerrainMapOctree: (vehN, vehE ) = (4061622.78, 594871.91). 2024-11-06T04:57:25Z- TerrainMapOctree: Min Distance = 176.20. 2024-11-06T04:57:25Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:57:25Z- Forcing Subcloud Comparison 2024-11-06T04:57:25Z- Weighting particles with subcloud comparison 2024-11-06T04:57:25Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:25Z- meanExpectedMeasDiff: 1.868052 delta_rms_squared: 0.999156 alpha: 0.980376 2024-11-06T04:57:25Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:25Z- meanExpectedMeasDiff: -1.816839 delta_rms_squared: 0.694540 alpha: 0.972010 2024-11-06T04:57:25Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:25Z- meanExpectedMeasDiff: -1.566031 delta_rms_squared: 0.811480 alpha: 0.975946 2024-11-06T04:57:25Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:25Z- meanExpectedMeasDiff: 0.036024 delta_rms_squared: 0.843614 alpha: 0.976842 2024-11-06T04:57:25Z- Subcloud NIS: 0.338729 2024-11-06T04:57:25Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:57:25Z- TNavPF:: sumWeights = 0.304863 2024-11-06T04:57:25Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:57:25Z- TNavPF::First Term of Map Covariance Matrix is 0.82 2024-11-06T04:57:25Z- TNavPF::Calculated NIS Value : 0.19 numBeams normalized NIS: 0.05 2024-11-06T04:57:25Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869045.04 sec, ping # 0. 2024-11-06T04:57:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:57:25Z- variances([0], [2], and psi_berg) = 0.705806 0.298453 0.000000 2024-11-06T04:57:25Z- publishEstimatesDecoder() - at 1730869045.04 sec 2024-11-06T04:57:25Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:57:25Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061566.68 594875.98 59.43 2024-11-06T04:57:25Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061566.19 594876.34 59.20 lat/lon: 36.70 -121.94 2024-11-06T04:57:25Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.71 0.30 0.36 0.00 2024-11-06T04:57:25Z- AHRS_M2 msg: 1730869045.41 epoch sec; seqNo:3082 2024-11-06T04:57:25Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.36 psi 2024-11-06T04:57:25Z- handleDepth()- Depth_Keller msg: 1730869045.42 epoch sec; seqNo:3287; depth 49.67 2024-11-06T04:57:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869045.44 epoch sec; seqNo:3287; 4061622.40 north; 594871.91 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:57:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869045.04 2024-11-06T04:57:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:25Z- waiting for 1730869049.04, time is 1730869045.44 2024-11-06T04:57:25Z- AHRS_M2 msg: 1730869045.82 epoch sec; seqNo:3083 2024-11-06T04:57:25Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.36 psi 2024-11-06T04:57:25Z- handleDepth()- Depth_Keller msg: 1730869045.83 epoch sec; seqNo:3288; depth 49.68 2024-11-06T04:57:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869045.86 epoch sec; seqNo:3288; 4061622.02 north; 594871.92 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T04:57:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869045.04 2024-11-06T04:57:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:25Z- waiting for 1730869049.04, time is 1730869045.86 2024-11-06T04:57:26Z- AHRS_M2 msg: 1730869046.22 epoch sec; seqNo:3084 2024-11-06T04:57:26Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.35 psi 2024-11-06T04:57:26Z- handleDepth()- Depth_Keller msg: 1730869046.23 epoch sec; seqNo:3289; depth 49.66 2024-11-06T04:57:26Z- handleDVL() - DVL msg epochMillisec: 1730869046.23 2024-11-06T04:57:26Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:26Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:26Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:26Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:57:26Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:57:26Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:57:26Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:57:26Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:57:26Z- handleDvl() - RDI_Pathfinder msg: 1730869046.23 sec; seqNo:1180; #beams:0 2024-11-06T04:57:26Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:57:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.11 , 0.05 2024-11-06T04:57:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869046.24 epoch sec; seqNo:3289; 4061622.02 north; 594871.92 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:57:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869045.04 2024-11-06T04:57:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:26Z- waiting for 1730869049.04, time is 1730869046.24 2024-11-06T04:57:26Z- AHRS_M2 msg: 1730869046.62 epoch sec; seqNo:3085 2024-11-06T04:57:26Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.35 psi 2024-11-06T04:57:26Z- handleDepth()- Depth_Keller msg: 1730869046.63 epoch sec; seqNo:3290; depth 49.67 2024-11-06T04:57:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869046.66 epoch sec; seqNo:3290; 4061621.63 north; 594870.70 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:57:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869045.04 2024-11-06T04:57:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:26Z- waiting for 1730869049.04, time is 1730869046.66 2024-11-06T04:57:27Z- AHRS_M2 msg: 1730869047.03 epoch sec; seqNo:3086 2024-11-06T04:57:27Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.35 psi 2024-11-06T04:57:27Z- handleDepth()- Depth_Keller msg: 1730869047.03 epoch sec; seqNo:3291; depth 49.67 2024-11-06T04:57:27Z- handleDVL() - DVL msg epochMillisec: 1730869047.06 2024-11-06T04:57:27Z- handleDvl() - RDI_Pathfinder msg: 1730869047.06 sec; seqNo:1181; #beams:4 2024-11-06T04:57:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.95 , 37.72 , 41.38 , 44.65 2024-11-06T04:57:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.07 , 0.08 2024-11-06T04:57:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869047.07 epoch sec; seqNo:3291; 4061621.25 north; 594870.71 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:57:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869045.04 2024-11-06T04:57:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:27Z- waiting for 1730869049.04, time is 1730869047.07 2024-11-06T04:57:27Z- AHRS_M2 msg: 1730869047.43 epoch sec; seqNo:3087 2024-11-06T04:57:27Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.35 psi 2024-11-06T04:57:27Z- handleDepth()- Depth_Keller msg: 1730869047.44 epoch sec; seqNo:3292; depth 49.66 2024-11-06T04:57:27Z- handleDVL() - DVL msg epochMillisec: 1730869047.44 2024-11-06T04:57:27Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:27Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:27Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:27Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:57:27Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:57:27Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:57:27Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:57:27Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:57:27Z- handleDvl() - RDI_Pathfinder msg: 1730869047.44 sec; seqNo:1182; #beams:0 2024-11-06T04:57:27Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:57:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.07 , 0.08 2024-11-06T04:57:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869047.45 epoch sec; seqNo:3292; 4061620.87 north; 594870.71 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:57:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869045.04 2024-11-06T04:57:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:27Z- waiting for 1730869049.04, time is 1730869047.45 2024-11-06T04:57:27Z- AHRS_M2 msg: 1730869047.84 epoch sec; seqNo:3088 2024-11-06T04:57:27Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.35 psi 2024-11-06T04:57:27Z- handleDepth()- Depth_Keller msg: 1730869047.86 epoch sec; seqNo:3293; depth 49.66 2024-11-06T04:57:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869047.87 epoch sec; seqNo:3293; 4061620.49 north; 594870.71 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:57:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869045.04 2024-11-06T04:57:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:28Z- waiting for 1730869049.04, time is 1730869047.87 2024-11-06T04:57:28Z- AHRS_M2 msg: 1730869048.24 epoch sec; seqNo:3089 2024-11-06T04:57:28Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.35 psi 2024-11-06T04:57:28Z- handleDepth()- Depth_Keller msg: 1730869048.25 epoch sec; seqNo:3294; depth 49.66 2024-11-06T04:57:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869048.26 epoch sec; seqNo:3294; 4061620.11 north; 594870.72 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:57:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869045.04 2024-11-06T04:57:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:28Z- waiting for 1730869049.04, time is 1730869048.26 2024-11-06T04:57:28Z- AHRS_M2 msg: 1730869048.64 epoch sec; seqNo:3090 2024-11-06T04:57:28Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.35 psi 2024-11-06T04:57:28Z- handleDepth()- Depth_Keller msg: 1730869048.65 epoch sec; seqNo:3295; depth 49.66 2024-11-06T04:57:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869048.66 epoch sec; seqNo:3295; 4061619.73 north; 594870.72 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:57:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869045.04 2024-11-06T04:57:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:28Z- waiting for 1730869049.04, time is 1730869048.66 2024-11-06T04:57:29Z- AHRS_M2 msg: 1730869049.05 epoch sec; seqNo:3091 2024-11-06T04:57:29Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.35 psi 2024-11-06T04:57:29Z- handleDepth()- Depth_Keller msg: 1730869049.06 epoch sec; seqNo:3296; depth 49.66 2024-11-06T04:57:29Z- handleDVL() - DVL msg epochMillisec: 1730869049.08 2024-11-06T04:57:29Z- handleDvl() - RDI_Pathfinder msg: 1730869049.08 sec; seqNo:1183; #beams:4 2024-11-06T04:57:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.15 , 37.31 , 41.38 , 45.05 2024-11-06T04:57:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.01 , 0.06 2024-11-06T04:57:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869049.09 epoch sec; seqNo:3296; 4061619.35 north; 594870.73 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:57:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869045.04 2024-11-06T04:57:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:29Z- waiting for 1730869049.04, time is 1730869049.09 2024-11-06T04:57:29Z- LcmTrn::updateTrn() >>>> pose time 1730869049.09, meas time 1730869049.08 <<<< 2024-11-06T04:57:29Z- LcmTrn::updateTrn() equating pose 1730869049.08 and meas times 1730869049.08 2024-11-06T04:57:29Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869049.08 2024-11-06T04:57:29Z- TerrainNav::motionUpdate() - currEstimate = (4061619, 594870.73, 49.66) 2024-11-06T04:57:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:57:29Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:57:29Z- TNavPF::Projecting Measurements 2024-11-06T04:57:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:57:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:57:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:57:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:57:29Z- TNavFilter::4 good beams. 2024-11-06T04:57:29Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:57:29Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:57:29Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:57:29Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:57:29Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:57:29Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:57:29Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:57:29Z- TerrainMapOctree: (vehN, vehE ) = (4061619.35, 594870.73). 2024-11-06T04:57:29Z- TerrainMapOctree: Min Distance = 177.89. 2024-11-06T04:57:29Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:57:29Z- Forcing Subcloud Comparison 2024-11-06T04:57:29Z- Weighting particles with subcloud comparison 2024-11-06T04:57:29Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:29Z- meanExpectedMeasDiff: 1.874527 delta_rms_squared: 1.010495 alpha: 0.980592 2024-11-06T04:57:29Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:29Z- meanExpectedMeasDiff: -1.898800 delta_rms_squared: 0.656470 alpha: 0.970435 2024-11-06T04:57:29Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:29Z- meanExpectedMeasDiff: -1.783417 delta_rms_squared: 0.805966 alpha: 0.975786 2024-11-06T04:57:29Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:29Z- meanExpectedMeasDiff: 0.504622 delta_rms_squared: 0.859007 alpha: 0.977247 2024-11-06T04:57:29Z- Subcloud NIS: 0.347026 2024-11-06T04:57:29Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:57:29Z- TNavPF:: sumWeights = 0.261632 2024-11-06T04:57:29Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:57:29Z- TNavPF::First Term of Map Covariance Matrix is 0.76 2024-11-06T04:57:29Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T04:57:29Z- TerrainNav::Resampling particle filter... 2024-11-06T04:57:29Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869049.08 sec, ping # 0. 2024-11-06T04:57:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:57:29Z- variances([0], [2], and psi_berg) = 0.698518 0.288666 0.000000 2024-11-06T04:57:29Z- publishEstimatesDecoder() - at 1730869049.08 sec 2024-11-06T04:57:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:57:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061564.67 594875.40 60.05 2024-11-06T04:57:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061562.80 594875.13 59.33 lat/lon: 36.69 -121.94 2024-11-06T04:57:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.29 0.37 0.00 2024-11-06T04:57:29Z- AHRS_M2 msg: 1730869049.45 epoch sec; seqNo:3092 2024-11-06T04:57:29Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.35 psi 2024-11-06T04:57:29Z- handleDepth()- Depth_Keller msg: 1730869049.46 epoch sec; seqNo:3297; depth 49.66 2024-11-06T04:57:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869049.48 epoch sec; seqNo:3297; 4061618.97 north; 594870.73 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:57:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869049.08 2024-11-06T04:57:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:29Z- waiting for 1730869053.08, time is 1730869049.48 2024-11-06T04:57:29Z- AHRS_M2 msg: 1730869049.86 epoch sec; seqNo:3093 2024-11-06T04:57:29Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.35 psi 2024-11-06T04:57:29Z- handleDepth()- Depth_Keller msg: 1730869049.86 epoch sec; seqNo:3298; depth 49.65 2024-11-06T04:57:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869049.87 epoch sec; seqNo:3298; 4061618.60 north; 594870.74 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:57:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869049.08 2024-11-06T04:57:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:30Z- waiting for 1730869053.08, time is 1730869049.87 2024-11-06T04:57:30Z- AHRS_M2 msg: 1730869050.26 epoch sec; seqNo:3094 2024-11-06T04:57:30Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.35 psi 2024-11-06T04:57:30Z- handleDepth()- Depth_Keller msg: 1730869050.27 epoch sec; seqNo:3299; depth 49.66 2024-11-06T04:57:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869050.27 epoch sec; seqNo:3299; 4061618.22 north; 594870.74 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:57:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869049.08 2024-11-06T04:57:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:30Z- waiting for 1730869053.08, time is 1730869050.27 2024-11-06T04:57:30Z- AHRS_M2 msg: 1730869050.67 epoch sec; seqNo:3095 2024-11-06T04:57:30Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.35 psi 2024-11-06T04:57:30Z- handleDepth()- Depth_Keller msg: 1730869050.67 epoch sec; seqNo:3300; depth 49.64 2024-11-06T04:57:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869050.70 epoch sec; seqNo:3300; 4061617.84 north; 594870.74 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:57:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869049.08 2024-11-06T04:57:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:30Z- waiting for 1730869053.08, time is 1730869050.70 2024-11-06T04:57:31Z- AHRS_M2 msg: 1730869051.07 epoch sec; seqNo:3096 2024-11-06T04:57:31Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.35 psi 2024-11-06T04:57:31Z- handleDepth()- Depth_Keller msg: 1730869051.07 epoch sec; seqNo:3301; depth 49.65 2024-11-06T04:57:31Z- handleDVL() - DVL msg epochMillisec: 1730869051.10 2024-11-06T04:57:31Z- handleDvl() - RDI_Pathfinder msg: 1730869051.10 sec; seqNo:1184; #beams:4 2024-11-06T04:57:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.76 , 36.70 , 41.38 , 45.05 2024-11-06T04:57:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , 0.01 2024-11-06T04:57:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869051.11 epoch sec; seqNo:3301; 4061617.46 north; 594870.75 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:57:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869049.08 2024-11-06T04:57:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:31Z- waiting for 1730869053.08, time is 1730869051.11 2024-11-06T04:57:31Z- AHRS_M2 msg: 1730869051.47 epoch sec; seqNo:3097 2024-11-06T04:57:31Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.35 psi 2024-11-06T04:57:31Z- handleDepth()- Depth_Keller msg: 1730869051.48 epoch sec; seqNo:3302; depth 49.66 2024-11-06T04:57:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869051.49 epoch sec; seqNo:3302; 4061617.08 north; 594870.75 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:57:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869049.08 2024-11-06T04:57:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:31Z- waiting for 1730869053.08, time is 1730869051.49 2024-11-06T04:57:31Z- AHRS_M2 msg: 1730869051.88 epoch sec; seqNo:3098 2024-11-06T04:57:31Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.35 psi 2024-11-06T04:57:31Z- handleDepth()- Depth_Keller msg: 1730869051.89 epoch sec; seqNo:3303; depth 49.65 2024-11-06T04:57:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869051.90 epoch sec; seqNo:3303; 4061616.70 north; 594870.76 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:57:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869049.08 2024-11-06T04:57:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:32Z- waiting for 1730869053.08, time is 1730869051.90 2024-11-06T04:57:32Z- AHRS_M2 msg: 1730869052.28 epoch sec; seqNo:3099 2024-11-06T04:57:32Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.35 psi 2024-11-06T04:57:32Z- handleDepth()- Depth_Keller msg: 1730869052.29 epoch sec; seqNo:3304; depth 49.65 2024-11-06T04:57:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869052.31 epoch sec; seqNo:3304; 4061616.31 north; 594869.54 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:57:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869049.08 2024-11-06T04:57:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:32Z- waiting for 1730869053.08, time is 1730869052.31 2024-11-06T04:57:32Z- AHRS_M2 msg: 1730869052.73 epoch sec; seqNo:3100 2024-11-06T04:57:32Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 3.35 psi 2024-11-06T04:57:32Z- handleDepth()- Depth_Keller msg: 1730869052.75 epoch sec; seqNo:3305; depth 49.66 2024-11-06T04:57:32Z- handleDVL() - DVL msg epochMillisec: 1730869052.78 2024-11-06T04:57:32Z- handleDvl() - RDI_Pathfinder msg: 1730869052.78 sec; seqNo:1185; #beams:4 2024-11-06T04:57:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.77 , 36.50 , 41.19 , 45.06 2024-11-06T04:57:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.07 , -0.01 2024-11-06T04:57:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869052.79 epoch sec; seqNo:3305; 4061615.93 north; 594869.54 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:57:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869049.08 2024-11-06T04:57:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:32Z- waiting for 1730869053.08, time is 1730869052.79 2024-11-06T04:57:33Z- AHRS_M2 msg: 1730869053.09 epoch sec; seqNo:3101 2024-11-06T04:57:33Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.35 psi 2024-11-06T04:57:33Z- handleDepth()- Depth_Keller msg: 1730869053.10 epoch sec; seqNo:3306; depth 49.65 2024-11-06T04:57:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869053.11 epoch sec; seqNo:3306; 4061615.93 north; 594869.54 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:57:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869049.08 2024-11-06T04:57:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:33Z- waiting for 1730869053.08, time is 1730869053.11 2024-11-06T04:57:33Z- LcmTrn::updateTrn() >>>> pose time 1730869053.11, meas time 1730869052.78 <<<< 2024-11-06T04:57:33Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869053.11 2024-11-06T04:57:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869052.78 sec, ping # 0; waiting for INS data more recent than 1730869049.08... 2024-11-06T04:57:33Z- TerrainNav::motionUpdate() - currEstimate = (4061616, 594869.54, 49.65) 2024-11-06T04:57:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:57:33Z- TerrainNav:: SubcloudNIS is 0.35, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:57:33Z- TNavPF::Projecting Measurements 2024-11-06T04:57:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:57:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:57:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:57:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:57:33Z- TNavFilter::4 good beams. 2024-11-06T04:57:33Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:57:33Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:57:33Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:57:33Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:57:33Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:57:33Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:57:33Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:57:33Z- TerrainMapOctree: (vehN, vehE ) = (4061616.21, 594869.64). 2024-11-06T04:57:33Z- TerrainMapOctree: Min Distance = 179.48. 2024-11-06T04:57:33Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:57:33Z- Forcing Subcloud Comparison 2024-11-06T04:57:33Z- Weighting particles with subcloud comparison 2024-11-06T04:57:33Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:33Z- meanExpectedMeasDiff: 2.291890 delta_rms_squared: 1.018357 alpha: 0.980739 2024-11-06T04:57:33Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:33Z- meanExpectedMeasDiff: -2.379042 delta_rms_squared: 0.643724 alpha: 0.969867 2024-11-06T04:57:33Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:33Z- meanExpectedMeasDiff: -1.721224 delta_rms_squared: 0.795618 alpha: 0.975479 2024-11-06T04:57:33Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:33Z- meanExpectedMeasDiff: 0.382208 delta_rms_squared: 0.863479 alpha: 0.977362 2024-11-06T04:57:33Z- Subcloud NIS: 0.356690 2024-11-06T04:57:33Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:57:33Z- TNavPF:: sumWeights = 0.181435 2024-11-06T04:57:33Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:57:33Z- TNavPF::First Term of Map Covariance Matrix is 0.87 2024-11-06T04:57:33Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.02 2024-11-06T04:57:33Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869052.78 sec. 2024-11-06T04:57:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:57:33Z- variances([0], [2], and psi_berg) = 0.706768 0.278659 0.000000 2024-11-06T04:57:33Z- publishEstimatesDecoder() - at 1730869053.11 sec 2024-11-06T04:57:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:57:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061559.84 594874.49 59.78 2024-11-06T04:57:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061559.37 594873.93 59.48 lat/lon: 36.69 -121.94 2024-11-06T04:57:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.71 0.28 0.61 0.00 2024-11-06T04:57:33Z- AHRS_M2 msg: 1730869053.49 epoch sec; seqNo:3102 2024-11-06T04:57:33Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.35 psi 2024-11-06T04:57:33Z- handleDepth()- Depth_Keller msg: 1730869053.50 epoch sec; seqNo:3307; depth 49.64 2024-11-06T04:57:33Z- handleDVL() - DVL msg epochMillisec: 1730869053.50 2024-11-06T04:57:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:57:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:57:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:57:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:57:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:57:33Z- handleDvl() - RDI_Pathfinder msg: 1730869053.50 sec; seqNo:1186; #beams:0 2024-11-06T04:57:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:57:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.07 , -0.01 2024-11-06T04:57:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869053.52 epoch sec; seqNo:3307; 4061615.55 north; 594869.55 east; 36.70 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T04:57:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869053.11 2024-11-06T04:57:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:33Z- waiting for 1730869057.11, time is 1730869053.52 2024-11-06T04:57:33Z- AHRS_M2 msg: 1730869053.90 epoch sec; seqNo:3103 2024-11-06T04:57:33Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.35 psi 2024-11-06T04:57:33Z- handleDepth()- Depth_Keller msg: 1730869053.91 epoch sec; seqNo:3308; depth 49.65 2024-11-06T04:57:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869053.92 epoch sec; seqNo:3308; 4061615.17 north; 594869.55 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:57:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869053.11 2024-11-06T04:57:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:34Z- waiting for 1730869057.11, time is 1730869053.92 2024-11-06T04:57:34Z- AHRS_M2 msg: 1730869054.31 epoch sec; seqNo:3104 2024-11-06T04:57:34Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.34 psi 2024-11-06T04:57:34Z- handleDepth()- Depth_Keller msg: 1730869054.31 epoch sec; seqNo:3309; depth 49.65 2024-11-06T04:57:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869054.32 epoch sec; seqNo:3309; 4061614.79 north; 594869.56 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:57:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869053.11 2024-11-06T04:57:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:34Z- waiting for 1730869057.11, time is 1730869054.32 2024-11-06T04:57:34Z- AHRS_M2 msg: 1730869054.76 epoch sec; seqNo:3105 2024-11-06T04:57:34Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.34 psi 2024-11-06T04:57:34Z- handleDepth()- Depth_Keller msg: 1730869054.76 epoch sec; seqNo:3310; depth 49.66 2024-11-06T04:57:34Z- handleDVL() - DVL msg epochMillisec: 1730869054.80 2024-11-06T04:57:34Z- handleDvl() - RDI_Pathfinder msg: 1730869054.80 sec; seqNo:1187; #beams:4 2024-11-06T04:57:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.56 , 36.29 , 40.77 , 45.46 2024-11-06T04:57:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.09 , 0.02 2024-11-06T04:57:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869054.84 epoch sec; seqNo:3310; 4061614.41 north; 594869.56 east; 36.70 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T04:57:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869053.11 2024-11-06T04:57:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:34Z- waiting for 1730869057.11, time is 1730869054.84 2024-11-06T04:57:35Z- AHRS_M2 msg: 1730869055.11 epoch sec; seqNo:3106 2024-11-06T04:57:35Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.34 psi 2024-11-06T04:57:35Z- handleDepth()- Depth_Keller msg: 1730869055.12 epoch sec; seqNo:3311; depth 49.67 2024-11-06T04:57:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869055.15 epoch sec; seqNo:3311; 4061614.04 north; 594869.57 east; 36.70 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T04:57:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869053.11 2024-11-06T04:57:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:35Z- waiting for 1730869057.11, time is 1730869055.15 2024-11-06T04:57:35Z- AHRS_M2 msg: 1730869055.52 epoch sec; seqNo:3107 2024-11-06T04:57:35Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.34 psi 2024-11-06T04:57:35Z- handleDepth()- Depth_Keller msg: 1730869055.52 epoch sec; seqNo:3312; depth 49.65 2024-11-06T04:57:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869055.62 epoch sec; seqNo:3312; 4061613.66 north; 594869.57 east; 36.70 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T04:57:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869053.11 2024-11-06T04:57:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:35Z- waiting for 1730869057.11, time is 1730869055.62 2024-11-06T04:57:35Z- AHRS_M2 msg: 1730869055.92 epoch sec; seqNo:3108 2024-11-06T04:57:35Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.34 psi 2024-11-06T04:57:35Z- handleDepth()- Depth_Keller msg: 1730869055.93 epoch sec; seqNo:3313; depth 49.68 2024-11-06T04:57:35Z- handleDVL() - DVL msg epochMillisec: 1730869055.94 2024-11-06T04:57:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:57:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:57:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:57:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:57:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:57:35Z- handleDvl() - RDI_Pathfinder msg: 1730869055.94 sec; seqNo:1188; #beams:0 2024-11-06T04:57:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:57:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.09 , 0.02 2024-11-06T04:57:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869055.94 epoch sec; seqNo:3313; 4061613.28 north; 594869.57 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T04:57:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869053.11 2024-11-06T04:57:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:36Z- waiting for 1730869057.11, time is 1730869055.94 2024-11-06T04:57:36Z- AHRS_M2 msg: 1730869056.32 epoch sec; seqNo:3109 2024-11-06T04:57:36Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.34 psi 2024-11-06T04:57:36Z- handleDepth()- Depth_Keller msg: 1730869056.33 epoch sec; seqNo:3314; depth 49.68 2024-11-06T04:57:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869056.35 epoch sec; seqNo:3314; 4061612.90 north; 594869.58 east; 36.70 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T04:57:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869053.11 2024-11-06T04:57:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:36Z- waiting for 1730869057.11, time is 1730869056.35 2024-11-06T04:57:36Z- AHRS_M2 msg: 1730869056.76 epoch sec; seqNo:3110 2024-11-06T04:57:36Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:57:36Z- handleDepth()- Depth_Keller msg: 1730869056.76 epoch sec; seqNo:3315; depth 49.69 2024-11-06T04:57:36Z- handleDVL() - DVL msg epochMillisec: 1730869056.79 2024-11-06T04:57:36Z- handleDvl() - RDI_Pathfinder msg: 1730869056.79 sec; seqNo:1189; #beams:4 2024-11-06T04:57:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.35 , 35.68 , 40.37 , 45.26 2024-11-06T04:57:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.10 , 0.05 2024-11-06T04:57:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869056.83 epoch sec; seqNo:3315; 4061612.52 north; 594869.58 east; 36.70 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T04:57:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869053.11 2024-11-06T04:57:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:36Z- waiting for 1730869057.11, time is 1730869056.83 2024-11-06T04:57:37Z- AHRS_M2 msg: 1730869057.13 epoch sec; seqNo:3111 2024-11-06T04:57:37Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:57:37Z- handleDepth()- Depth_Keller msg: 1730869057.13 epoch sec; seqNo:3316; depth 49.71 2024-11-06T04:57:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869057.15 epoch sec; seqNo:3316; 4061612.51 north; 594868.36 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T04:57:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869053.11 2024-11-06T04:57:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:37Z- waiting for 1730869057.11, time is 1730869057.15 2024-11-06T04:57:37Z- LcmTrn::updateTrn() >>>> pose time 1730869057.15, meas time 1730869056.79 <<<< 2024-11-06T04:57:37Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869057.15 2024-11-06T04:57:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869056.79 sec, ping # 0; waiting for INS data more recent than 1730869053.11... 2024-11-06T04:57:37Z- TerrainNav::motionUpdate() - currEstimate = (4061613, 594868.36, 49.71) 2024-11-06T04:57:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:57:37Z- TerrainNav:: SubcloudNIS is 0.36, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:57:37Z- TNavPF::Projecting Measurements 2024-11-06T04:57:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:57:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:57:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:57:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:57:37Z- TNavFilter::4 good beams. 2024-11-06T04:57:37Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:57:37Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:57:37Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:57:37Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:57:37Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:57:37Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:57:37Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:57:37Z- TerrainMapOctree: (vehN, vehE ) = (4061612.82, 594868.47). 2024-11-06T04:57:37Z- TerrainMapOctree: Min Distance = 181.26. 2024-11-06T04:57:37Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:57:37Z- Forcing Subcloud Comparison 2024-11-06T04:57:37Z- Weighting particles with subcloud comparison 2024-11-06T04:57:37Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:37Z- meanExpectedMeasDiff: 2.269619 delta_rms_squared: 1.429035 alpha: 0.986198 2024-11-06T04:57:37Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:37Z- meanExpectedMeasDiff: -2.997747 delta_rms_squared: 0.973095 alpha: 0.979861 2024-11-06T04:57:37Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:37Z- meanExpectedMeasDiff: -2.155870 delta_rms_squared: 1.115252 alpha: 0.982383 2024-11-06T04:57:37Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:37Z- meanExpectedMeasDiff: 0.693336 delta_rms_squared: 1.226023 alpha: 0.983949 2024-11-06T04:57:37Z- Subcloud NIS: 0.377964 2024-11-06T04:57:37Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:57:37Z- TNavPF:: sumWeights = 0.089451 2024-11-06T04:57:37Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:57:37Z- TNavPF::First Term of Map Covariance Matrix is 0.59 2024-11-06T04:57:37Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T04:57:37Z- TerrainNav::Resampling particle filter... 2024-11-06T04:57:37Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869056.79 sec. 2024-11-06T04:57:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:57:37Z- variances([0], [2], and psi_berg) = 0.704802 0.280183 0.000000 2024-11-06T04:57:37Z- publishEstimatesDecoder() - at 1730869057.15 sec 2024-11-06T04:57:37Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:57:37Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061557.76 594873.05 60.01 2024-11-06T04:57:37Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061555.94 594872.74 59.84 lat/lon: 36.69 -121.94 2024-11-06T04:57:37Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.28 0.68 0.00 2024-11-06T04:57:37Z- AHRS_M2 msg: 1730869057.54 epoch sec; seqNo:3112 2024-11-06T04:57:37Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:57:37Z- handleDepth()- Depth_Keller msg: 1730869057.55 epoch sec; seqNo:3317; depth 49.71 2024-11-06T04:57:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869057.56 epoch sec; seqNo:3317; 4061612.13 north; 594868.37 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T04:57:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869057.15 2024-11-06T04:57:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:37Z- waiting for 1730869061.15, time is 1730869057.56 2024-11-06T04:57:37Z- AHRS_M2 msg: 1730869057.96 epoch sec; seqNo:3113 2024-11-06T04:57:37Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.33 psi 2024-11-06T04:57:37Z- handleDepth()- Depth_Keller msg: 1730869057.96 epoch sec; seqNo:3318; depth 49.71 2024-11-06T04:57:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869057.97 epoch sec; seqNo:3318; 4061611.75 north; 594868.37 east; 36.70 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T04:57:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869057.15 2024-11-06T04:57:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:38Z- waiting for 1730869061.15, time is 1730869057.97 2024-11-06T04:57:38Z- AHRS_M2 msg: 1730869058.35 epoch sec; seqNo:3114 2024-11-06T04:57:38Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.33 psi 2024-11-06T04:57:38Z- handleDepth()- Depth_Keller msg: 1730869058.37 epoch sec; seqNo:3319; depth 49.73 2024-11-06T04:57:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869058.38 epoch sec; seqNo:3319; 4061611.37 north; 594868.37 east; 36.70 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T04:57:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869057.15 2024-11-06T04:57:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:38Z- waiting for 1730869061.15, time is 1730869058.38 2024-11-06T04:57:38Z- AHRS_M2 msg: 1730869058.76 epoch sec; seqNo:3115 2024-11-06T04:57:38Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.33 psi 2024-11-06T04:57:38Z- handleDepth()- Depth_Keller msg: 1730869058.77 epoch sec; seqNo:3320; depth 49.75 2024-11-06T04:57:38Z- handleDVL() - DVL msg epochMillisec: 1730869058.79 2024-11-06T04:57:38Z- handleDvl() - RDI_Pathfinder msg: 1730869058.79 sec; seqNo:1190; #beams:4 2024-11-06T04:57:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.56 , 35.88 , 39.96 , 45.26 2024-11-06T04:57:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.06 , 0.07 2024-11-06T04:57:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869058.82 epoch sec; seqNo:3320; 4061610.99 north; 594868.38 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:57:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869057.15 2024-11-06T04:57:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:38Z- waiting for 1730869061.15, time is 1730869058.82 2024-11-06T04:57:39Z- AHRS_M2 msg: 1730869059.15 epoch sec; seqNo:3116 2024-11-06T04:57:39Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.33 psi 2024-11-06T04:57:39Z- handleDepth()- Depth_Keller msg: 1730869059.15 epoch sec; seqNo:3321; depth 49.75 2024-11-06T04:57:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869059.18 epoch sec; seqNo:3321; 4061610.61 north; 594868.38 east; 36.70 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:57:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869057.15 2024-11-06T04:57:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:39Z- waiting for 1730869061.15, time is 1730869059.18 2024-11-06T04:57:39Z- AHRS_M2 msg: 1730869059.56 epoch sec; seqNo:3117 2024-11-06T04:57:39Z- AHRS_M2 msg: 0.18 phi; -0.04 theta; 3.33 psi 2024-11-06T04:57:39Z- handleDepth()- Depth_Keller msg: 1730869059.56 epoch sec; seqNo:3322; depth 49.76 2024-11-06T04:57:39Z- handleDVL() - DVL msg epochMillisec: 1730869059.57 2024-11-06T04:57:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:57:39Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:57:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:57:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:57:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:57:39Z- handleDvl() - RDI_Pathfinder msg: 1730869059.57 sec; seqNo:1191; #beams:0 2024-11-06T04:57:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:57:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.06 , 0.07 2024-11-06T04:57:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869059.57 epoch sec; seqNo:3322; 4061610.23 north; 594868.39 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T04:57:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869057.15 2024-11-06T04:57:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:39Z- waiting for 1730869061.15, time is 1730869059.57 2024-11-06T04:57:39Z- AHRS_M2 msg: 1730869059.96 epoch sec; seqNo:3118 2024-11-06T04:57:39Z- AHRS_M2 msg: 0.18 phi; -0.04 theta; 3.33 psi 2024-11-06T04:57:39Z- handleDepth()- Depth_Keller msg: 1730869059.96 epoch sec; seqNo:3323; depth 49.76 2024-11-06T04:57:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869059.98 epoch sec; seqNo:3323; 4061609.86 north; 594868.39 east; 36.70 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T04:57:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869057.15 2024-11-06T04:57:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:40Z- waiting for 1730869061.15, time is 1730869059.98 2024-11-06T04:57:40Z- AHRS_M2 msg: 1730869060.36 epoch sec; seqNo:3119 2024-11-06T04:57:40Z- AHRS_M2 msg: 0.18 phi; -0.04 theta; 3.33 psi 2024-11-06T04:57:40Z- handleDepth()- Depth_Keller msg: 1730869060.37 epoch sec; seqNo:3324; depth 49.79 2024-11-06T04:57:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869060.38 epoch sec; seqNo:3324; 4061609.48 north; 594868.40 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:57:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869057.15 2024-11-06T04:57:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:40Z- waiting for 1730869061.15, time is 1730869060.38 2024-11-06T04:57:40Z- AHRS_M2 msg: 1730869060.77 epoch sec; seqNo:3120 2024-11-06T04:57:40Z- AHRS_M2 msg: 0.18 phi; -0.05 theta; 3.34 psi 2024-11-06T04:57:40Z- handleDepth()- Depth_Keller msg: 1730869060.77 epoch sec; seqNo:3325; depth 49.78 2024-11-06T04:57:40Z- handleDVL() - DVL msg epochMillisec: 1730869060.80 2024-11-06T04:57:40Z- handleDvl() - RDI_Pathfinder msg: 1730869060.80 sec; seqNo:1192; #beams:4 2024-11-06T04:57:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.97 , 35.68 , 40.37 , 45.67 2024-11-06T04:57:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , 0.06 2024-11-06T04:57:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869060.82 epoch sec; seqNo:3325; 4061609.10 north; 594868.40 east; 36.70 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:57:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869057.15 2024-11-06T04:57:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:40Z- waiting for 1730869061.15, time is 1730869060.82 2024-11-06T04:57:41Z- AHRS_M2 msg: 1730869061.19 epoch sec; seqNo:3121 2024-11-06T04:57:41Z- AHRS_M2 msg: 0.17 phi; -0.05 theta; 3.34 psi 2024-11-06T04:57:41Z- handleDepth()- Depth_Keller msg: 1730869061.19 epoch sec; seqNo:3326; depth 49.79 2024-11-06T04:57:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869061.22 epoch sec; seqNo:3326; 4061608.72 north; 594868.40 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:57:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869057.15 2024-11-06T04:57:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:41Z- waiting for 1730869061.15, time is 1730869061.22 2024-11-06T04:57:41Z- LcmTrn::updateTrn() >>>> pose time 1730869061.22, meas time 1730869060.80 <<<< 2024-11-06T04:57:41Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869061.22 2024-11-06T04:57:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869060.80 sec, ping # 0; waiting for INS data more recent than 1730869057.15... 2024-11-06T04:57:41Z- TerrainNav::motionUpdate() - currEstimate = (4061609, 594868.40, 49.79) 2024-11-06T04:57:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:57:41Z- TerrainNav:: SubcloudNIS is 0.38, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:57:41Z- TNavPF::Projecting Measurements 2024-11-06T04:57:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:57:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:57:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:57:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:57:41Z- TNavFilter::4 good beams. 2024-11-06T04:57:41Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:57:41Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:57:41Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:57:41Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:57:41Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:57:41Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:57:41Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:57:41Z- TerrainMapOctree: (vehN, vehE ) = (4061609.11, 594868.40). 2024-11-06T04:57:41Z- TerrainMapOctree: Min Distance = 184.03. 2024-11-06T04:57:41Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:57:41Z- Forcing Subcloud Comparison 2024-11-06T04:57:41Z- Weighting particles with subcloud comparison 2024-11-06T04:57:41Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:41Z- meanExpectedMeasDiff: 2.985389 delta_rms_squared: 1.578869 alpha: 0.987491 2024-11-06T04:57:41Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:41Z- meanExpectedMeasDiff: -2.432103 delta_rms_squared: 1.049082 alpha: 0.981292 2024-11-06T04:57:41Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:41Z- meanExpectedMeasDiff: -1.651443 delta_rms_squared: 1.208400 alpha: 0.983719 2024-11-06T04:57:41Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:41Z- meanExpectedMeasDiff: 1.357319 delta_rms_squared: 1.344952 alpha: 0.985347 2024-11-06T04:57:41Z- Subcloud NIS: 0.397261 2024-11-06T04:57:41Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:57:41Z- TNavPF:: sumWeights = 0.093674 2024-11-06T04:57:41Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:57:41Z- TNavPF::First Term of Map Covariance Matrix is 0.79 2024-11-06T04:57:41Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T04:57:41Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869060.80 sec. 2024-11-06T04:57:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:57:41Z- variances([0], [2], and psi_berg) = 0.700481 0.280594 0.000000 2024-11-06T04:57:41Z- publishEstimatesDecoder() - at 1730869061.22 sec 2024-11-06T04:57:41Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:57:41Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061551.71 594872.83 60.03 2024-11-06T04:57:41Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061552.15 594872.78 59.99 lat/lon: 36.69 -121.94 2024-11-06T04:57:41Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.28 0.70 0.00 2024-11-06T04:57:41Z- AHRS_M2 msg: 1730869061.62 epoch sec; seqNo:3122 2024-11-06T04:57:41Z- AHRS_M2 msg: 0.17 phi; -0.05 theta; 3.34 psi 2024-11-06T04:57:41Z- handleDepth()- Depth_Keller msg: 1730869061.63 epoch sec; seqNo:3327; depth 49.79 2024-11-06T04:57:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869061.64 epoch sec; seqNo:3327; 4061608.34 north; 594868.41 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:57:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869061.22 2024-11-06T04:57:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:41Z- waiting for 1730869065.22, time is 1730869061.64 2024-11-06T04:57:41Z- AHRS_M2 msg: 1730869061.98 epoch sec; seqNo:3123 2024-11-06T04:57:41Z- AHRS_M2 msg: 0.17 phi; -0.05 theta; 3.34 psi 2024-11-06T04:57:41Z- handleDepth()- Depth_Keller msg: 1730869061.99 epoch sec; seqNo:3328; depth 49.81 2024-11-06T04:57:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869062.00 epoch sec; seqNo:3328; 4061607.96 north; 594868.41 east; 36.70 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:57:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869061.22 2024-11-06T04:57:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:42Z- waiting for 1730869065.22, time is 1730869062.00 2024-11-06T04:57:42Z- AHRS_M2 msg: 1730869062.38 epoch sec; seqNo:3124 2024-11-06T04:57:42Z- AHRS_M2 msg: 0.17 phi; -0.05 theta; 3.34 psi 2024-11-06T04:57:42Z- handleDepth()- Depth_Keller msg: 1730869062.39 epoch sec; seqNo:3329; depth 49.80 2024-11-06T04:57:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869062.40 epoch sec; seqNo:3329; 4061607.96 north; 594868.41 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T04:57:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869061.22 2024-11-06T04:57:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:42Z- waiting for 1730869065.22, time is 1730869062.40 2024-11-06T04:57:42Z- AHRS_M2 msg: 1730869062.79 epoch sec; seqNo:3125 2024-11-06T04:57:42Z- AHRS_M2 msg: 0.17 phi; -0.05 theta; 3.35 psi 2024-11-06T04:57:42Z- handleDepth()- Depth_Keller msg: 1730869062.79 epoch sec; seqNo:3330; depth 49.80 2024-11-06T04:57:42Z- handleDVL() - DVL msg epochMillisec: 1730869062.83 2024-11-06T04:57:42Z- handleDvl() - RDI_Pathfinder msg: 1730869062.83 sec; seqNo:1193; #beams:4 2024-11-06T04:57:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.97 , 35.47 , 39.96 , 45.67 2024-11-06T04:57:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , 0.02 2024-11-06T04:57:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869062.84 epoch sec; seqNo:3330; 4061607.19 north; 594867.20 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T04:57:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869061.22 2024-11-06T04:57:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:42Z- waiting for 1730869065.22, time is 1730869062.84 2024-11-06T04:57:43Z- AHRS_M2 msg: 1730869063.19 epoch sec; seqNo:3126 2024-11-06T04:57:43Z- AHRS_M2 msg: 0.17 phi; -0.05 theta; 3.35 psi 2024-11-06T04:57:43Z- handleDepth()- Depth_Keller msg: 1730869063.20 epoch sec; seqNo:3331; depth 49.80 2024-11-06T04:57:43Z- handleDVL() - DVL msg epochMillisec: 1730869063.20 2024-11-06T04:57:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:57:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:57:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:57:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:57:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:57:43Z- handleDvl() - RDI_Pathfinder msg: 1730869063.20 sec; seqNo:1194; #beams:0 2024-11-06T04:57:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:57:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , 0.02 2024-11-06T04:57:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869063.21 epoch sec; seqNo:3331; 4061607.19 north; 594867.20 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T04:57:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869061.22 2024-11-06T04:57:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:43Z- waiting for 1730869065.22, time is 1730869063.21 2024-11-06T04:57:43Z- AHRS_M2 msg: 1730869063.60 epoch sec; seqNo:3127 2024-11-06T04:57:43Z- AHRS_M2 msg: 0.17 phi; -0.05 theta; 3.35 psi 2024-11-06T04:57:43Z- handleDepth()- Depth_Keller msg: 1730869063.61 epoch sec; seqNo:3332; depth 49.79 2024-11-06T04:57:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869063.62 epoch sec; seqNo:3332; 4061606.81 north; 594867.20 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:57:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869061.22 2024-11-06T04:57:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:43Z- waiting for 1730869065.22, time is 1730869063.62 2024-11-06T04:57:44Z- handleDepth()- Depth_Keller msg: 1730869064.02 epoch sec; seqNo:3333; depth 49.80 2024-11-06T04:57:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869064.03 epoch sec; seqNo:3333; 4061606.43 north; 594867.21 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T04:57:44Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:57:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869061.22 2024-11-06T04:57:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:44Z- waiting for 1730869065.22, time is 1730869064.03 2024-11-06T04:57:44Z- AHRS_M2 msg: 1730869064.40 epoch sec; seqNo:3128 2024-11-06T04:57:44Z- AHRS_M2 msg: 0.17 phi; -0.05 theta; 3.36 psi 2024-11-06T04:57:44Z- handleDepth()- Depth_Keller msg: 1730869064.41 epoch sec; seqNo:3334; depth 49.80 2024-11-06T04:57:44Z- handleDVL() - DVL msg epochMillisec: 1730869064.41 2024-11-06T04:57:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:57:44Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:57:44Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:57:44Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:57:44Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:57:44Z- handleDvl() - RDI_Pathfinder msg: 1730869064.41 sec; seqNo:1195; #beams:0 2024-11-06T04:57:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:57:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , 0.02 2024-11-06T04:57:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869064.42 epoch sec; seqNo:3334; 4061606.05 north; 594867.21 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T04:57:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869061.22 2024-11-06T04:57:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:44Z- waiting for 1730869065.22, time is 1730869064.42 2024-11-06T04:57:44Z- AHRS_M2 msg: 1730869064.82 epoch sec; seqNo:3129 2024-11-06T04:57:44Z- AHRS_M2 msg: 0.17 phi; -0.05 theta; 3.36 psi 2024-11-06T04:57:44Z- handleDepth()- Depth_Keller msg: 1730869064.83 epoch sec; seqNo:3335; depth 49.80 2024-11-06T04:57:44Z- handleDVL() - DVL msg epochMillisec: 1730869064.85 2024-11-06T04:57:44Z- handleDvl() - RDI_Pathfinder msg: 1730869064.85 sec; seqNo:1196; #beams:4 2024-11-06T04:57:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.97 , 35.48 , 39.35 , 45.47 2024-11-06T04:57:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.03 , -0.03 2024-11-06T04:57:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869064.87 epoch sec; seqNo:3335; 4061605.67 north; 594867.22 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T04:57:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869061.22 2024-11-06T04:57:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:44Z- waiting for 1730869065.22, time is 1730869064.87 2024-11-06T04:57:45Z- AHRS_M2 msg: 1730869065.21 epoch sec; seqNo:3130 2024-11-06T04:57:45Z- AHRS_M2 msg: 0.17 phi; -0.05 theta; 3.36 psi 2024-11-06T04:57:45Z- handleDepth()- Depth_Keller msg: 1730869065.22 epoch sec; seqNo:3336; depth 49.80 2024-11-06T04:57:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869065.24 epoch sec; seqNo:3336; 4061605.30 north; 594867.22 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T04:57:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869061.22 2024-11-06T04:57:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:45Z- waiting for 1730869065.22, time is 1730869065.24 2024-11-06T04:57:45Z- LcmTrn::updateTrn() >>>> pose time 1730869065.24, meas time 1730869064.85 <<<< 2024-11-06T04:57:45Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869065.24 2024-11-06T04:57:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869064.85 sec, ping # 0; waiting for INS data more recent than 1730869061.22... 2024-11-06T04:57:45Z- TerrainNav::motionUpdate() - currEstimate = (4061605, 594867.22, 49.80) 2024-11-06T04:57:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:57:45Z- TerrainNav:: SubcloudNIS is 0.40, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:57:45Z- TNavPF::Projecting Measurements 2024-11-06T04:57:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:57:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:57:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:57:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:57:45Z- TNavFilter::4 good beams. 2024-11-06T04:57:45Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:57:45Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:57:45Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:57:45Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:57:45Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:57:45Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:57:45Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:57:45Z- TerrainMapOctree: (vehN, vehE ) = (4061605.63, 594867.34). 2024-11-06T04:57:45Z- TerrainMapOctree: Min Distance = 186.04. 2024-11-06T04:57:45Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:57:45Z- Forcing Subcloud Comparison 2024-11-06T04:57:45Z- Weighting particles with subcloud comparison 2024-11-06T04:57:45Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:45Z- meanExpectedMeasDiff: 3.154090 delta_rms_squared: 1.603189 alpha: 0.987679 2024-11-06T04:57:45Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:45Z- meanExpectedMeasDiff: -2.589202 delta_rms_squared: 1.070357 alpha: 0.981657 2024-11-06T04:57:45Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:45Z- meanExpectedMeasDiff: -2.466365 delta_rms_squared: 1.223663 alpha: 0.983918 2024-11-06T04:57:45Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:45Z- meanExpectedMeasDiff: 1.127281 delta_rms_squared: 1.377043 alpha: 0.985684 2024-11-06T04:57:45Z- Subcloud NIS: 0.422695 2024-11-06T04:57:45Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:57:45Z- TNavPF:: sumWeights = 0.056225 2024-11-06T04:57:45Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:57:45Z- TNavPF::First Term of Map Covariance Matrix is 0.62 2024-11-06T04:57:45Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T04:57:45Z- TerrainNav::Resampling particle filter... 2024-11-06T04:57:45Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869064.85 sec. 2024-11-06T04:57:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:57:45Z- variances([0], [2], and psi_berg) = 0.700213 0.278395 0.000000 2024-11-06T04:57:45Z- publishEstimatesDecoder() - at 1730869065.24 sec 2024-11-06T04:57:45Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:57:45Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061550.46 594871.87 60.61 2024-11-06T04:57:45Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061548.73 594871.60 60.17 lat/lon: 36.69 -121.94 2024-11-06T04:57:45Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.28 0.70 0.00 2024-11-06T04:57:45Z- AHRS_M2 msg: 1730869065.62 epoch sec; seqNo:3131 2024-11-06T04:57:45Z- AHRS_M2 msg: 0.17 phi; -0.05 theta; 3.36 psi 2024-11-06T04:57:45Z- handleDepth()- Depth_Keller msg: 1730869065.62 epoch sec; seqNo:3337; depth 49.79 2024-11-06T04:57:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869065.63 epoch sec; seqNo:3337; 4061604.92 north; 594867.23 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:57:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869065.24 2024-11-06T04:57:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:45Z- waiting for 1730869069.24, time is 1730869065.63 2024-11-06T04:57:46Z- AHRS_M2 msg: 1730869066.02 epoch sec; seqNo:3132 2024-11-06T04:57:46Z- AHRS_M2 msg: 0.17 phi; -0.05 theta; 3.36 psi 2024-11-06T04:57:46Z- handleDepth()- Depth_Keller msg: 1730869066.03 epoch sec; seqNo:3338; depth 49.80 2024-11-06T04:57:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869066.04 epoch sec; seqNo:3338; 4061604.54 north; 594867.23 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T04:57:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869065.24 2024-11-06T04:57:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:46Z- waiting for 1730869069.24, time is 1730869066.04 2024-11-06T04:57:46Z- AHRS_M2 msg: 1730869066.42 epoch sec; seqNo:3133 2024-11-06T04:57:46Z- AHRS_M2 msg: 0.17 phi; -0.05 theta; 3.36 psi 2024-11-06T04:57:46Z- handleDepth()- Depth_Keller msg: 1730869066.43 epoch sec; seqNo:3339; depth 49.79 2024-11-06T04:57:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869066.44 epoch sec; seqNo:3339; 4061604.16 north; 594867.23 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:57:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869065.24 2024-11-06T04:57:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:46Z- waiting for 1730869069.24, time is 1730869066.44 2024-11-06T04:57:46Z- AHRS_M2 msg: 1730869066.83 epoch sec; seqNo:3134 2024-11-06T04:57:46Z- AHRS_M2 msg: 0.17 phi; -0.05 theta; 3.37 psi 2024-11-06T04:57:46Z- handleDepth()- Depth_Keller msg: 1730869066.84 epoch sec; seqNo:3340; depth 49.80 2024-11-06T04:57:46Z- handleDVL() - DVL msg epochMillisec: 1730869066.86 2024-11-06T04:57:46Z- handleDvl() - RDI_Pathfinder msg: 1730869066.86 sec; seqNo:1197; #beams:4 2024-11-06T04:57:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.97 , 34.25 , 38.74 , 45.06 2024-11-06T04:57:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.11 , -0.04 2024-11-06T04:57:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869066.87 epoch sec; seqNo:3340; 4061603.78 north; 594867.24 east; 36.70 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T04:57:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869065.24 2024-11-06T04:57:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:46Z- waiting for 1730869069.24, time is 1730869066.87 2024-11-06T04:57:47Z- AHRS_M2 msg: 1730869067.23 epoch sec; seqNo:3135 2024-11-06T04:57:47Z- AHRS_M2 msg: 0.17 phi; -0.05 theta; 3.37 psi 2024-11-06T04:57:47Z- handleDepth()- Depth_Keller msg: 1730869067.24 epoch sec; seqNo:3341; depth 49.79 2024-11-06T04:57:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869067.26 epoch sec; seqNo:3341; 4061603.40 north; 594867.24 east; 36.70 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:57:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869065.24 2024-11-06T04:57:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:47Z- waiting for 1730869069.24, time is 1730869067.26 2024-11-06T04:57:47Z- AHRS_M2 msg: 1730869067.63 epoch sec; seqNo:3136 2024-11-06T04:57:47Z- AHRS_M2 msg: 0.17 phi; -0.05 theta; 3.37 psi 2024-11-06T04:57:47Z- handleDepth()- Depth_Keller msg: 1730869067.64 epoch sec; seqNo:3342; depth 49.82 2024-11-06T04:57:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869067.65 epoch sec; seqNo:3342; 4061603.02 north; 594867.25 east; 36.70 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:57:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869065.24 2024-11-06T04:57:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:47Z- waiting for 1730869069.24, time is 1730869067.65 2024-11-06T04:57:48Z- AHRS_M2 msg: 1730869068.04 epoch sec; seqNo:3137 2024-11-06T04:57:48Z- AHRS_M2 msg: 0.17 phi; -0.06 theta; 3.37 psi 2024-11-06T04:57:48Z- handleDepth()- Depth_Keller msg: 1730869068.04 epoch sec; seqNo:3343; depth 49.83 2024-11-06T04:57:48Z- handleDVL() - DVL msg epochMillisec: 1730869068.05 2024-11-06T04:57:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:57:48Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:57:48Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:57:48Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:57:48Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:57:48Z- handleDvl() - RDI_Pathfinder msg: 1730869068.05 sec; seqNo:1198; #beams:0 2024-11-06T04:57:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:57:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.11 , -0.04 2024-11-06T04:57:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869068.06 epoch sec; seqNo:3343; 4061602.63 north; 594866.03 east; 36.70 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:57:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869065.24 2024-11-06T04:57:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:48Z- waiting for 1730869069.24, time is 1730869068.06 2024-11-06T04:57:48Z- AHRS_M2 msg: 1730869068.44 epoch sec; seqNo:3138 2024-11-06T04:57:48Z- AHRS_M2 msg: 0.17 phi; -0.06 theta; 3.37 psi 2024-11-06T04:57:48Z- handleDepth()- Depth_Keller msg: 1730869068.45 epoch sec; seqNo:3344; depth 49.84 2024-11-06T04:57:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869068.46 epoch sec; seqNo:3344; 4061602.25 north; 594866.03 east; 36.70 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T04:57:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869065.24 2024-11-06T04:57:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:48Z- waiting for 1730869069.24, time is 1730869068.46 2024-11-06T04:57:48Z- AHRS_M2 msg: 1730869068.85 epoch sec; seqNo:3139 2024-11-06T04:57:48Z- AHRS_M2 msg: 0.17 phi; -0.06 theta; 3.37 psi 2024-11-06T04:57:48Z- handleDepth()- Depth_Keller msg: 1730869068.85 epoch sec; seqNo:3345; depth 49.86 2024-11-06T04:57:48Z- handleDVL() - DVL msg epochMillisec: 1730869068.87 2024-11-06T04:57:48Z- handleDvl() - RDI_Pathfinder msg: 1730869068.87 sec; seqNo:1199; #beams:4 2024-11-06T04:57:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.36 , 34.87 , 38.53 , 45.06 2024-11-06T04:57:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.18 , 0.02 2024-11-06T04:57:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869068.90 epoch sec; seqNo:3345; 4061601.87 north; 594866.04 east; 36.70 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:57:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869065.24 2024-11-06T04:57:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:49Z- waiting for 1730869069.24, time is 1730869068.90 2024-11-06T04:57:49Z- AHRS_M2 msg: 1730869069.25 epoch sec; seqNo:3140 2024-11-06T04:57:49Z- AHRS_M2 msg: 0.17 phi; -0.06 theta; 3.37 psi 2024-11-06T04:57:49Z- handleDepth()- Depth_Keller msg: 1730869069.27 epoch sec; seqNo:3346; depth 49.87 2024-11-06T04:57:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869069.28 epoch sec; seqNo:3346; 4061601.87 north; 594866.04 east; 36.70 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:57:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869065.24 2024-11-06T04:57:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:49Z- waiting for 1730869069.24, time is 1730869069.28 2024-11-06T04:57:49Z- LcmTrn::updateTrn() >>>> pose time 1730869069.28, meas time 1730869068.87 <<<< 2024-11-06T04:57:49Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869069.28 2024-11-06T04:57:49Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869068.87 sec, ping # 0; waiting for INS data more recent than 1730869065.24... 2024-11-06T04:57:49Z- TerrainNav::motionUpdate() - currEstimate = (4061602, 594866.04, 49.87) 2024-11-06T04:57:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:57:49Z- TerrainNav:: SubcloudNIS is 0.42, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:57:49Z- TNavPF::Projecting Measurements 2024-11-06T04:57:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:57:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:57:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:57:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:57:49Z- TNavFilter::4 good beams. 2024-11-06T04:57:49Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:57:49Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:57:49Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:57:49Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:57:49Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:57:49Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:57:49Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:57:49Z- TerrainMapOctree: (vehN, vehE ) = (4061602.22, 594866.16). 2024-11-06T04:57:49Z- TerrainMapOctree: Min Distance = 187.97. 2024-11-06T04:57:49Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:57:49Z- Forcing Subcloud Comparison 2024-11-06T04:57:49Z- Weighting particles with subcloud comparison 2024-11-06T04:57:49Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:49Z- meanExpectedMeasDiff: 2.858478 delta_rms_squared: 1.592504 alpha: 0.987597 2024-11-06T04:57:49Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:49Z- meanExpectedMeasDiff: -2.990098 delta_rms_squared: 1.065940 alpha: 0.981583 2024-11-06T04:57:49Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:49Z- meanExpectedMeasDiff: -2.892716 delta_rms_squared: 1.208894 alpha: 0.983725 2024-11-06T04:57:49Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:49Z- meanExpectedMeasDiff: 0.822044 delta_rms_squared: 1.379807 alpha: 0.985712 2024-11-06T04:57:49Z- Subcloud NIS: 0.451556 2024-11-06T04:57:49Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:57:49Z- TNavPF:: sumWeights = 0.040989 2024-11-06T04:57:49Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:57:49Z- TNavPF::First Term of Map Covariance Matrix is 0.45 2024-11-06T04:57:49Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T04:57:49Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869068.87 sec. 2024-11-06T04:57:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:57:49Z- variances([0], [2], and psi_berg) = 0.697557 0.279595 0.000000 2024-11-06T04:57:49Z- publishEstimatesDecoder() - at 1730869069.28 sec 2024-11-06T04:57:49Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:57:49Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061543.56 594870.93 61.57 2024-11-06T04:57:49Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061545.30 594870.42 60.59 lat/lon: 36.69 -121.94 2024-11-06T04:57:49Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.28 0.70 0.00 2024-11-06T04:57:49Z- AHRS_M2 msg: 1730869069.65 epoch sec; seqNo:3141 2024-11-06T04:57:49Z- AHRS_M2 msg: 0.17 phi; -0.06 theta; 3.37 psi 2024-11-06T04:57:49Z- handleDepth()- Depth_Keller msg: 1730869069.66 epoch sec; seqNo:3347; depth 49.89 2024-11-06T04:57:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869069.67 epoch sec; seqNo:3347; 4061601.49 north; 594866.04 east; 36.70 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:57:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869069.28 2024-11-06T04:57:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:49Z- waiting for 1730869073.28, time is 1730869069.67 2024-11-06T04:57:50Z- AHRS_M2 msg: 1730869070.06 epoch sec; seqNo:3142 2024-11-06T04:57:50Z- AHRS_M2 msg: 0.17 phi; -0.06 theta; 3.36 psi 2024-11-06T04:57:50Z- handleDepth()- Depth_Keller msg: 1730869070.07 epoch sec; seqNo:3348; depth 49.91 2024-11-06T04:57:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869070.08 epoch sec; seqNo:3348; 4061601.11 north; 594866.05 east; 36.70 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:57:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869069.28 2024-11-06T04:57:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:50Z- waiting for 1730869073.28, time is 1730869070.08 2024-11-06T04:57:50Z- AHRS_M2 msg: 1730869070.46 epoch sec; seqNo:3143 2024-11-06T04:57:50Z- AHRS_M2 msg: 0.17 phi; -0.06 theta; 3.36 psi 2024-11-06T04:57:50Z- handleDepth()- Depth_Keller msg: 1730869070.47 epoch sec; seqNo:3349; depth 49.92 2024-11-06T04:57:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869070.48 epoch sec; seqNo:3349; 4061600.74 north; 594866.05 east; 36.70 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:57:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869069.28 2024-11-06T04:57:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:50Z- waiting for 1730869073.28, time is 1730869070.48 2024-11-06T04:57:50Z- AHRS_M2 msg: 1730869070.87 epoch sec; seqNo:3144 2024-11-06T04:57:50Z- AHRS_M2 msg: 0.17 phi; -0.06 theta; 3.36 psi 2024-11-06T04:57:50Z- handleDepth()- Depth_Keller msg: 1730869070.87 epoch sec; seqNo:3350; depth 49.95 2024-11-06T04:57:50Z- handleDVL() - DVL msg epochMillisec: 1730869070.90 2024-11-06T04:57:50Z- handleDvl() - RDI_Pathfinder msg: 1730869070.90 sec; seqNo:1200; #beams:4 2024-11-06T04:57:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.16 , 34.46 , 38.13 , 45.06 2024-11-06T04:57:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.14 , 0.11 2024-11-06T04:57:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869070.92 epoch sec; seqNo:3350; 4061600.36 north; 594866.06 east; 36.70 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T04:57:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869069.28 2024-11-06T04:57:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:51Z- waiting for 1730869073.28, time is 1730869070.92 2024-11-06T04:57:51Z- AHRS_M2 msg: 1730869071.27 epoch sec; seqNo:3145 2024-11-06T04:57:51Z- AHRS_M2 msg: 0.17 phi; -0.06 theta; 3.37 psi 2024-11-06T04:57:51Z- handleDepth()- Depth_Keller msg: 1730869071.28 epoch sec; seqNo:3351; depth 49.97 2024-11-06T04:57:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869071.29 epoch sec; seqNo:3351; 4061599.98 north; 594866.06 east; 36.70 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T04:57:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869069.28 2024-11-06T04:57:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:51Z- waiting for 1730869073.28, time is 1730869071.29 2024-11-06T04:57:51Z- AHRS_M2 msg: 1730869071.78 epoch sec; seqNo:3146 2024-11-06T04:57:51Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.37 psi 2024-11-06T04:57:51Z- handleDepth()- Depth_Keller msg: 1730869071.78 epoch sec; seqNo:3352; depth 50.00 2024-11-06T04:57:51Z- handleDVL() - DVL msg epochMillisec: 1730869071.79 2024-11-06T04:57:51Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:51Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:51Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:57:51Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:57:51Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:57:51Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:57:51Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:57:51Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:57:51Z- handleDvl() - RDI_Pathfinder msg: 1730869071.79 sec; seqNo:1201; #beams:0 2024-11-06T04:57:51Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:57:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.14 , 0.11 2024-11-06T04:57:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869071.79 epoch sec; seqNo:3352; 4061599.59 north; 594864.84 east; 36.70 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T04:57:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869069.28 2024-11-06T04:57:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:51Z- waiting for 1730869073.28, time is 1730869071.79 2024-11-06T04:57:52Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:57:52Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:57:52Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:57:52Z- AHRS_M2 msg: 1730869072.08 epoch sec; seqNo:3147 2024-11-06T04:57:52Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.37 psi 2024-11-06T04:57:52Z- handleDepth()- Depth_Keller msg: 1730869072.11 epoch sec; seqNo:3353; depth 50.02 2024-11-06T04:57:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869072.12 epoch sec; seqNo:3353; 4061599.59 north; 594864.84 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:57:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869069.28 2024-11-06T04:57:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:52Z- waiting for 1730869073.28, time is 1730869072.12 2024-11-06T04:57:52Z- AHRS_M2 msg: 1730869072.48 epoch sec; seqNo:3148 2024-11-06T04:57:52Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.37 psi 2024-11-06T04:57:52Z- handleDepth()- Depth_Keller msg: 1730869072.49 epoch sec; seqNo:3354; depth 50.02 2024-11-06T04:57:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869072.50 epoch sec; seqNo:3354; 4061599.21 north; 594864.85 east; 36.70 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:57:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869069.28 2024-11-06T04:57:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:52Z- waiting for 1730869073.28, time is 1730869072.50 2024-11-06T04:57:52Z- handleDepth()- Depth_Keller msg: 1730869072.91 epoch sec; seqNo:3355; depth 50.06 2024-11-06T04:57:52Z- handleDVL() - DVL msg epochMillisec: 1730869072.93 2024-11-06T04:57:52Z- handleDvl() - RDI_Pathfinder msg: 1730869072.93 sec; seqNo:1202; #beams:4 2024-11-06T04:57:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.95 , 34.25 , 37.72 , 45.47 2024-11-06T04:57:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , 0.12 2024-11-06T04:57:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869072.95 epoch sec; seqNo:3355; 4061598.83 north; 594864.85 east; 36.70 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T04:57:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869069.28 2024-11-06T04:57:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:53Z- waiting for 1730869073.28, time is 1730869072.95 2024-11-06T04:57:53Z- AHRS_M2 msg: 1730869073.29 epoch sec; seqNo:3149 2024-11-06T04:57:53Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.37 psi 2024-11-06T04:57:53Z- handleDepth()- Depth_Keller msg: 1730869073.30 epoch sec; seqNo:3356; depth 50.08 2024-11-06T04:57:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869073.31 epoch sec; seqNo:3356; 4061598.45 north; 594864.86 east; 36.70 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T04:57:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869069.28 2024-11-06T04:57:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:53Z- waiting for 1730869073.28, time is 1730869073.31 2024-11-06T04:57:53Z- LcmTrn::updateTrn() >>>> pose time 1730869073.31, meas time 1730869072.93 <<<< 2024-11-06T04:57:53Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869073.31 2024-11-06T04:57:53Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869072.93 sec, ping # 0; waiting for INS data more recent than 1730869069.28... 2024-11-06T04:57:53Z- TerrainNav::motionUpdate() - currEstimate = (4061598, 594864.86, 50.08) 2024-11-06T04:57:53Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:57:53Z- TerrainNav:: SubcloudNIS is 0.45, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:57:53Z- TNavPF::Projecting Measurements 2024-11-06T04:57:53Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:57:53Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:57:53Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:57:53Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:57:53Z- TNavFilter::4 good beams. 2024-11-06T04:57:53Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:57:53Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:57:53Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:57:53Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:57:53Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:57:53Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:57:53Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:57:53Z- TerrainMapOctree: (vehN, vehE ) = (4061598.77, 594864.97). 2024-11-06T04:57:53Z- TerrainMapOctree: Min Distance = 189.97. 2024-11-06T04:57:53Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:57:53Z- Forcing Subcloud Comparison 2024-11-06T04:57:53Z- Weighting particles with subcloud comparison 2024-11-06T04:57:53Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:53Z- meanExpectedMeasDiff: 3.498305 delta_rms_squared: 1.594118 alpha: 0.987609 2024-11-06T04:57:53Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:53Z- meanExpectedMeasDiff: -3.142410 delta_rms_squared: 1.089926 alpha: 0.981981 2024-11-06T04:57:53Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:53Z- meanExpectedMeasDiff: -2.965896 delta_rms_squared: 1.220445 alpha: 0.983877 2024-11-06T04:57:53Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:53Z- meanExpectedMeasDiff: 1.883086 delta_rms_squared: 1.402044 alpha: 0.985936 2024-11-06T04:57:53Z- Subcloud NIS: 0.480252 2024-11-06T04:57:53Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:57:53Z- TNavPF:: sumWeights = 0.015821 2024-11-06T04:57:53Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:57:53Z- TNavPF::First Term of Map Covariance Matrix is 0.23 2024-11-06T04:57:53Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:57:53Z- TerrainNav::Resampling particle filter... 2024-11-06T04:57:53Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869072.93 sec. 2024-11-06T04:57:53Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:57:53Z- variances([0], [2], and psi_berg) = 0.694892 0.281913 0.000000 2024-11-06T04:57:53Z- publishEstimatesDecoder() - at 1730869073.31 sec 2024-11-06T04:57:53Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:57:53Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061543.62 594869.51 61.21 2024-11-06T04:57:53Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061541.87 594869.24 61.03 lat/lon: 36.69 -121.94 2024-11-06T04:57:53Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.69 0.28 0.68 0.00 2024-11-06T04:57:53Z- AHRS_M2 msg: 1730869073.69 epoch sec; seqNo:3150 2024-11-06T04:57:53Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.37 psi 2024-11-06T04:57:53Z- handleDepth()- Depth_Keller msg: 1730869073.70 epoch sec; seqNo:3357; depth 50.10 2024-11-06T04:57:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869073.74 epoch sec; seqNo:3357; 4061598.07 north; 594864.86 east; 36.70 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T04:57:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869073.31 2024-11-06T04:57:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:53Z- waiting for 1730869077.31, time is 1730869073.74 2024-11-06T04:57:54Z- AHRS_M2 msg: 1730869074.10 epoch sec; seqNo:3151 2024-11-06T04:57:54Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.38 psi 2024-11-06T04:57:54Z- handleDepth()- Depth_Keller msg: 1730869074.11 epoch sec; seqNo:3358; depth 50.13 2024-11-06T04:57:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869074.12 epoch sec; seqNo:3358; 4061597.69 north; 594864.86 east; 36.70 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T04:57:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869073.31 2024-11-06T04:57:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:54Z- waiting for 1730869077.31, time is 1730869074.12 2024-11-06T04:57:54Z- AHRS_M2 msg: 1730869074.50 epoch sec; seqNo:3152 2024-11-06T04:57:54Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.38 psi 2024-11-06T04:57:54Z- handleDepth()- Depth_Keller msg: 1730869074.52 epoch sec; seqNo:3359; depth 50.14 2024-11-06T04:57:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869074.53 epoch sec; seqNo:3359; 4061597.31 north; 594864.87 east; 36.70 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:57:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869073.31 2024-11-06T04:57:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:54Z- waiting for 1730869077.31, time is 1730869074.53 2024-11-06T04:57:54Z- AHRS_M2 msg: 1730869074.91 epoch sec; seqNo:3153 2024-11-06T04:57:54Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.38 psi 2024-11-06T04:57:54Z- handleDepth()- Depth_Keller msg: 1730869074.91 epoch sec; seqNo:3360; depth 50.15 2024-11-06T04:57:54Z- handleDVL() - DVL msg epochMillisec: 1730869074.95 2024-11-06T04:57:54Z- handleDvl() - RDI_Pathfinder msg: 1730869074.95 sec; seqNo:1203; #beams:4 2024-11-06T04:57:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.75 , 34.46 , 36.90 , 45.26 2024-11-06T04:57:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.03 , 0.05 2024-11-06T04:57:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869074.96 epoch sec; seqNo:3360; 4061596.93 north; 594864.87 east; 36.70 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T04:57:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869073.31 2024-11-06T04:57:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:55Z- waiting for 1730869077.31, time is 1730869074.96 2024-11-06T04:57:55Z- AHRS_M2 msg: 1730869075.31 epoch sec; seqNo:3154 2024-11-06T04:57:55Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.39 psi 2024-11-06T04:57:55Z- handleDepth()- Depth_Keller msg: 1730869075.32 epoch sec; seqNo:3361; depth 50.16 2024-11-06T04:57:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869075.34 epoch sec; seqNo:3361; 4061596.56 north; 594864.88 east; 36.70 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T04:57:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869073.31 2024-11-06T04:57:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:55Z- waiting for 1730869077.31, time is 1730869075.34 2024-11-06T04:57:55Z- AHRS_M2 msg: 1730869075.72 epoch sec; seqNo:3155 2024-11-06T04:57:55Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.39 psi 2024-11-06T04:57:55Z- handleDepth()- Depth_Keller msg: 1730869075.72 epoch sec; seqNo:3362; depth 50.19 2024-11-06T04:57:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869075.73 epoch sec; seqNo:3362; 4061596.18 north; 594864.88 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:57:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869073.31 2024-11-06T04:57:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:55Z- waiting for 1730869077.31, time is 1730869075.73 2024-11-06T04:57:56Z- AHRS_M2 msg: 1730869076.12 epoch sec; seqNo:3156 2024-11-06T04:57:56Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.39 psi 2024-11-06T04:57:56Z- handleDepth()- Depth_Keller msg: 1730869076.13 epoch sec; seqNo:3363; depth 50.19 2024-11-06T04:57:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869076.15 epoch sec; seqNo:3363; 4061595.80 north; 594864.89 east; 36.70 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:57:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869073.31 2024-11-06T04:57:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:56Z- waiting for 1730869077.31, time is 1730869076.15 2024-11-06T04:57:56Z- AHRS_M2 msg: 1730869076.52 epoch sec; seqNo:3157 2024-11-06T04:57:56Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.39 psi 2024-11-06T04:57:56Z- handleDepth()- Depth_Keller msg: 1730869076.54 epoch sec; seqNo:3364; depth 50.20 2024-11-06T04:57:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869076.54 epoch sec; seqNo:3364; 4061595.42 north; 594864.89 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:57:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869073.31 2024-11-06T04:57:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:56Z- waiting for 1730869077.31, time is 1730869076.54 2024-11-06T04:57:56Z- AHRS_M2 msg: 1730869076.93 epoch sec; seqNo:3158 2024-11-06T04:57:56Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.39 psi 2024-11-06T04:57:56Z- handleDepth()- Depth_Keller msg: 1730869076.95 epoch sec; seqNo:3365; depth 50.21 2024-11-06T04:57:56Z- handleDVL() - DVL msg epochMillisec: 1730869076.98 2024-11-06T04:57:56Z- handleDvl() - RDI_Pathfinder msg: 1730869076.98 sec; seqNo:1204; #beams:4 2024-11-06T04:57:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.28 , 34.20 , 39.44 , 45.23 2024-11-06T04:57:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.01 , -0.04 2024-11-06T04:57:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869077.00 epoch sec; seqNo:3365; 4061595.03 north; 594863.67 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T04:57:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869073.31 2024-11-06T04:57:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:57Z- waiting for 1730869077.31, time is 1730869077.00 2024-11-06T04:57:57Z- AHRS_M2 msg: 1730869077.33 epoch sec; seqNo:3159 2024-11-06T04:57:57Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.39 psi 2024-11-06T04:57:57Z- handleDepth()- Depth_Keller msg: 1730869077.34 epoch sec; seqNo:3366; depth 50.20 2024-11-06T04:57:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869077.35 epoch sec; seqNo:3366; 4061594.65 north; 594863.68 east; 36.70 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:57:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869073.31 2024-11-06T04:57:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:57Z- waiting for 1730869077.31, time is 1730869077.35 2024-11-06T04:57:57Z- LcmTrn::updateTrn() >>>> pose time 1730869077.35, meas time 1730869076.98 <<<< 2024-11-06T04:57:57Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869077.35 2024-11-06T04:57:57Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869076.98 sec, ping # 0; waiting for INS data more recent than 1730869073.31... 2024-11-06T04:57:57Z- TerrainNav::motionUpdate() - currEstimate = (4061595, 594863.68, 50.20) 2024-11-06T04:57:57Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:57:57Z- TerrainNav:: SubcloudNIS is 0.48, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:57:57Z- TNavPF::Projecting Measurements 2024-11-06T04:57:57Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:57:57Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:57:57Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:57:57Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:57:57Z- TNavFilter::4 good beams. 2024-11-06T04:57:57Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:57:57Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:57:57Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:57:57Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:57:57Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:57:57Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:57:57Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:57:57Z- TerrainMapOctree: (vehN, vehE ) = (4061594.99, 594863.78). 2024-11-06T04:57:57Z- TerrainMapOctree: Min Distance = 192.29. 2024-11-06T04:57:57Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:57:57Z- Forcing Subcloud Comparison 2024-11-06T04:57:57Z- Weighting particles with subcloud comparison 2024-11-06T04:57:57Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:57Z- meanExpectedMeasDiff: 4.436368 delta_rms_squared: 1.555510 alpha: 0.987306 2024-11-06T04:57:57Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:57Z- meanExpectedMeasDiff: -2.849150 delta_rms_squared: 1.072901 alpha: 0.981700 2024-11-06T04:57:57Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:57Z- meanExpectedMeasDiff: -0.762105 delta_rms_squared: 1.193877 alpha: 0.983524 2024-11-06T04:57:57Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:57:57Z- meanExpectedMeasDiff: 2.066880 delta_rms_squared: 1.378027 alpha: 0.985694 2024-11-06T04:57:57Z- Subcloud NIS: 0.503443 2024-11-06T04:57:57Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:57:57Z- TNavPF:: sumWeights = 0.026990 2024-11-06T04:57:57Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:57:57Z- TNavPF::First Term of Map Covariance Matrix is 0.47 2024-11-06T04:57:57Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T04:57:57Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869076.98 sec. 2024-11-06T04:57:57Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:57:57Z- variances([0], [2], and psi_berg) = 0.696388 0.284216 0.000000 2024-11-06T04:57:57Z- publishEstimatesDecoder() - at 1730869077.35 sec 2024-11-06T04:57:57Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:57:57Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061538.58 594867.60 61.72 2024-11-06T04:57:57Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061538.07 594868.07 60.96 lat/lon: 36.69 -121.94 2024-11-06T04:57:57Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.28 0.71 0.00 2024-11-06T04:57:57Z- AHRS_M2 msg: 1730869077.73 epoch sec; seqNo:3160 2024-11-06T04:57:57Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.39 psi 2024-11-06T04:57:57Z- handleDepth()- Depth_Keller msg: 1730869077.75 epoch sec; seqNo:3367; depth 50.21 2024-11-06T04:57:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869077.77 epoch sec; seqNo:3367; 4061594.27 north; 594863.68 east; 36.70 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T04:57:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869077.35 2024-11-06T04:57:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:57Z- waiting for 1730869081.35, time is 1730869077.77 2024-11-06T04:57:58Z- AHRS_M2 msg: 1730869078.14 epoch sec; seqNo:3161 2024-11-06T04:57:58Z- AHRS_M2 msg: 0.16 phi; -0.06 theta; 3.39 psi 2024-11-06T04:57:58Z- handleDepth()- Depth_Keller msg: 1730869078.15 epoch sec; seqNo:3368; depth 50.23 2024-11-06T04:57:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869078.16 epoch sec; seqNo:3368; 4061594.27 north; 594863.68 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T04:57:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869077.35 2024-11-06T04:57:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:58Z- waiting for 1730869081.35, time is 1730869078.16 2024-11-06T04:57:58Z- AHRS_M2 msg: 1730869078.54 epoch sec; seqNo:3162 2024-11-06T04:57:58Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.39 psi 2024-11-06T04:57:58Z- handleDepth()- Depth_Keller msg: 1730869078.55 epoch sec; seqNo:3369; depth 50.24 2024-11-06T04:57:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869078.56 epoch sec; seqNo:3369; 4061593.89 north; 594863.69 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:57:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869077.35 2024-11-06T04:57:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:58Z- waiting for 1730869081.35, time is 1730869078.56 2024-11-06T04:57:58Z- AHRS_M2 msg: 1730869078.95 epoch sec; seqNo:3163 2024-11-06T04:57:58Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.39 psi 2024-11-06T04:57:58Z- handleDepth()- Depth_Keller msg: 1730869078.95 epoch sec; seqNo:3370; depth 50.24 2024-11-06T04:57:59Z- handleDVL() - DVL msg epochMillisec: 1730869078.99 2024-11-06T04:57:59Z- handleDvl() - RDI_Pathfinder msg: 1730869078.99 sec; seqNo:1205; #beams:4 2024-11-06T04:57:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.34 , 32.83 , 38.94 , 44.65 2024-11-06T04:57:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.07 , -0.05 2024-11-06T04:57:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869079.00 epoch sec; seqNo:3370; 4061593.51 north; 594863.69 east; 36.70 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:57:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:57:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869077.35 2024-11-06T04:57:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:59Z- waiting for 1730869081.35, time is 1730869079.00 2024-11-06T04:57:59Z- AHRS_M2 msg: 1730869079.36 epoch sec; seqNo:3164 2024-11-06T04:57:59Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.39 psi 2024-11-06T04:57:59Z- handleDepth()- Depth_Keller msg: 1730869079.36 epoch sec; seqNo:3371; depth 50.23 2024-11-06T04:57:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869079.39 epoch sec; seqNo:3371; 4061593.13 north; 594863.69 east; 36.70 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T04:57:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869077.35 2024-11-06T04:57:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:59Z- waiting for 1730869081.35, time is 1730869079.39 2024-11-06T04:57:59Z- AHRS_M2 msg: 1730869079.76 epoch sec; seqNo:3165 2024-11-06T04:57:59Z- AHRS_M2 msg: 0.15 phi; -0.06 theta; 3.38 psi 2024-11-06T04:57:59Z- handleDepth()- Depth_Keller msg: 1730869079.76 epoch sec; seqNo:3372; depth 50.25 2024-11-06T04:57:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869079.78 epoch sec; seqNo:3372; 4061592.75 north; 594863.70 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T04:57:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:57:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869077.35 2024-11-06T04:57:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:57:59Z- waiting for 1730869081.35, time is 1730869079.78 2024-11-06T04:58:00Z- AHRS_M2 msg: 1730869080.17 epoch sec; seqNo:3166 2024-11-06T04:58:00Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.38 psi 2024-11-06T04:58:00Z- handleDepth()- Depth_Keller msg: 1730869080.17 epoch sec; seqNo:3373; depth 50.25 2024-11-06T04:58:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869080.18 epoch sec; seqNo:3373; 4061592.37 north; 594863.70 east; 36.70 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T04:58:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869077.35 2024-11-06T04:58:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:00Z- waiting for 1730869081.35, time is 1730869080.18 2024-11-06T04:58:00Z- handleDepth()- Depth_Keller msg: 1730869080.59 epoch sec; seqNo:3374; depth 50.26 2024-11-06T04:58:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869080.60 epoch sec; seqNo:3374; 4061592.00 north; 594863.71 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:58:00Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:58:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869077.35 2024-11-06T04:58:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:00Z- waiting for 1730869081.35, time is 1730869080.60 2024-11-06T04:58:00Z- AHRS_M2 msg: 1730869080.97 epoch sec; seqNo:3167 2024-11-06T04:58:00Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.38 psi 2024-11-06T04:58:01Z- handleDepth()- Depth_Keller msg: 1730869081.00 epoch sec; seqNo:3375; depth 50.26 2024-11-06T04:58:01Z- handleDVL() - DVL msg epochMillisec: 1730869081.02 2024-11-06T04:58:01Z- handleDvl() - RDI_Pathfinder msg: 1730869081.02 sec; seqNo:1206; #beams:4 2024-11-06T04:58:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.97 , 32.52 , 38.87 , 44.29 2024-11-06T04:58:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.14 , -0.00 2024-11-06T04:58:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869081.03 epoch sec; seqNo:3375; 4061591.62 north; 594863.71 east; 36.70 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:58:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869077.35 2024-11-06T04:58:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:01Z- waiting for 1730869081.35, time is 1730869081.03 2024-11-06T04:58:01Z- AHRS_M2 msg: 1730869081.37 epoch sec; seqNo:3168 2024-11-06T04:58:01Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.38 psi 2024-11-06T04:58:01Z- handleDepth()- Depth_Keller msg: 1730869081.38 epoch sec; seqNo:3376; depth 50.28 2024-11-06T04:58:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869081.40 epoch sec; seqNo:3376; 4061591.22 north; 594862.50 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T04:58:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869077.35 2024-11-06T04:58:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:01Z- waiting for 1730869081.35, time is 1730869081.40 2024-11-06T04:58:01Z- LcmTrn::updateTrn() >>>> pose time 1730869081.40, meas time 1730869081.02 <<<< 2024-11-06T04:58:01Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869081.40 2024-11-06T04:58:01Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869081.02 sec, ping # 0; waiting for INS data more recent than 1730869077.35... 2024-11-06T04:58:01Z- TerrainNav::motionUpdate() - currEstimate = (4061591, 594862.50, 50.28) 2024-11-06T04:58:01Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:58:01Z- TerrainNav:: SubcloudNIS is 0.50, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:58:01Z- TNavPF::Projecting Measurements 2024-11-06T04:58:01Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:58:01Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:58:01Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:58:01Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:58:01Z- TNavFilter::4 good beams. 2024-11-06T04:58:01Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:58:01Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:58:01Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:58:01Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:58:01Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:58:01Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:58:01Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:58:01Z- TerrainMapOctree: (vehN, vehE ) = (4061591.54, 594862.61). 2024-11-06T04:58:01Z- TerrainMapOctree: Min Distance = 194.39. 2024-11-06T04:58:01Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:58:01Z- Forcing Subcloud Comparison 2024-11-06T04:58:01Z- Weighting particles with subcloud comparison 2024-11-06T04:58:01Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:01Z- meanExpectedMeasDiff: 3.240854 delta_rms_squared: 1.573044 alpha: 0.987445 2024-11-06T04:58:01Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:01Z- meanExpectedMeasDiff: -4.726100 delta_rms_squared: 1.101836 alpha: 0.982172 2024-11-06T04:58:01Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:01Z- meanExpectedMeasDiff: -1.554794 delta_rms_squared: 1.227501 alpha: 0.983968 2024-11-06T04:58:01Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:01Z- meanExpectedMeasDiff: 1.245735 delta_rms_squared: 1.393056 alpha: 0.985846 2024-11-06T04:58:01Z- Subcloud NIS: 0.527752 2024-11-06T04:58:01Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:58:01Z- TNavPF:: sumWeights = 0.008097 2024-11-06T04:58:01Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:58:01Z- TNavPF::First Term of Map Covariance Matrix is 0.12 2024-11-06T04:58:01Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:58:01Z- TerrainNav::Resampling particle filter... 2024-11-06T04:58:01Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869081.02 sec. 2024-11-06T04:58:01Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:58:01Z- variances([0], [2], and psi_berg) = 0.703157 0.291222 0.000000 2024-11-06T04:58:01Z- publishEstimatesDecoder() - at 1730869081.40 sec 2024-11-06T04:58:01Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:58:01Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061536.34 594867.02 59.44 2024-11-06T04:58:01Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061534.65 594866.88 61.40 lat/lon: 36.69 -121.94 2024-11-06T04:58:01Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.29 0.70 0.00 2024-11-06T04:58:01Z- AHRS_M2 msg: 1730869081.78 epoch sec; seqNo:3169 2024-11-06T04:58:01Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.38 psi 2024-11-06T04:58:01Z- handleDepth()- Depth_Keller msg: 1730869081.79 epoch sec; seqNo:3377; depth 50.29 2024-11-06T04:58:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869081.82 epoch sec; seqNo:3377; 4061590.85 north; 594862.50 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:58:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869081.40 2024-11-06T04:58:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:01Z- waiting for 1730869085.40, time is 1730869081.82 2024-11-06T04:58:02Z- AHRS_M2 msg: 1730869082.18 epoch sec; seqNo:3170 2024-11-06T04:58:02Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.37 psi 2024-11-06T04:58:02Z- handleDepth()- Depth_Keller msg: 1730869082.20 epoch sec; seqNo:3378; depth 50.28 2024-11-06T04:58:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869082.21 epoch sec; seqNo:3378; 4061590.47 north; 594862.50 east; 36.70 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T04:58:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869081.40 2024-11-06T04:58:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:02Z- waiting for 1730869085.40, time is 1730869082.21 2024-11-06T04:58:02Z- AHRS_M2 msg: 1730869082.59 epoch sec; seqNo:3171 2024-11-06T04:58:02Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.37 psi 2024-11-06T04:58:02Z- handleDepth()- Depth_Keller msg: 1730869082.59 epoch sec; seqNo:3379; depth 50.31 2024-11-06T04:58:02Z- handleDVL() - DVL msg epochMillisec: 1730869082.59 2024-11-06T04:58:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:58:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:58:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:58:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:58:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:58:02Z- handleDvl() - RDI_Pathfinder msg: 1730869082.59 sec; seqNo:1207; #beams:0 2024-11-06T04:58:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:58:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.14 , -0.00 2024-11-06T04:58:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869082.61 epoch sec; seqNo:3379; 4061590.09 north; 594862.51 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:58:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869081.40 2024-11-06T04:58:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:02Z- waiting for 1730869085.40, time is 1730869082.61 2024-11-06T04:58:03Z- AHRS_M2 msg: 1730869083.00 epoch sec; seqNo:3172 2024-11-06T04:58:03Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.37 psi 2024-11-06T04:58:03Z- handleDepth()- Depth_Keller msg: 1730869083.01 epoch sec; seqNo:3380; depth 50.31 2024-11-06T04:58:03Z- handleDVL() - DVL msg epochMillisec: 1730869083.03 2024-11-06T04:58:03Z- handleDvl() - RDI_Pathfinder msg: 1730869083.03 sec; seqNo:1208; #beams:4 2024-11-06T04:58:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.59 , 32.52 , 38.31 , 43.92 2024-11-06T04:58:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.15 , 0.05 2024-11-06T04:58:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869083.05 epoch sec; seqNo:3380; 4061590.09 north; 594862.51 east; 36.70 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:58:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869081.40 2024-11-06T04:58:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:03Z- waiting for 1730869085.40, time is 1730869083.05 2024-11-06T04:58:03Z- AHRS_M2 msg: 1730869083.39 epoch sec; seqNo:3173 2024-11-06T04:58:03Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.37 psi 2024-11-06T04:58:03Z- handleDepth()- Depth_Keller msg: 1730869083.40 epoch sec; seqNo:3381; depth 50.33 2024-11-06T04:58:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869083.41 epoch sec; seqNo:3381; 4061589.71 north; 594862.51 east; 36.70 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T04:58:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869081.40 2024-11-06T04:58:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:03Z- waiting for 1730869085.40, time is 1730869083.41 2024-11-06T04:58:03Z- AHRS_M2 msg: 1730869083.79 epoch sec; seqNo:3174 2024-11-06T04:58:03Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.37 psi 2024-11-06T04:58:03Z- handleDepth()- Depth_Keller msg: 1730869083.80 epoch sec; seqNo:3382; depth 50.34 2024-11-06T04:58:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869083.82 epoch sec; seqNo:3382; 4061589.33 north; 594862.52 east; 36.70 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:58:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869081.40 2024-11-06T04:58:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:03Z- waiting for 1730869085.40, time is 1730869083.82 2024-11-06T04:58:04Z- AHRS_M2 msg: 1730869084.20 epoch sec; seqNo:3175 2024-11-06T04:58:04Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.37 psi 2024-11-06T04:58:04Z- handleDepth()- Depth_Keller msg: 1730869084.21 epoch sec; seqNo:3383; depth 50.35 2024-11-06T04:58:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869084.23 epoch sec; seqNo:3383; 4061588.95 north; 594862.52 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:58:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869081.40 2024-11-06T04:58:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:04Z- waiting for 1730869085.40, time is 1730869084.23 2024-11-06T04:58:04Z- AHRS_M2 msg: 1730869084.61 epoch sec; seqNo:3176 2024-11-06T04:58:04Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.37 psi 2024-11-06T04:58:04Z- handleDepth()- Depth_Keller msg: 1730869084.61 epoch sec; seqNo:3384; depth 50.38 2024-11-06T04:58:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869084.62 epoch sec; seqNo:3384; 4061588.57 north; 594862.52 east; 36.70 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:58:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869081.40 2024-11-06T04:58:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:04Z- waiting for 1730869085.40, time is 1730869084.62 2024-11-06T04:58:05Z- AHRS_M2 msg: 1730869085.01 epoch sec; seqNo:3177 2024-11-06T04:58:05Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.37 psi 2024-11-06T04:58:05Z- handleDepth()- Depth_Keller msg: 1730869085.02 epoch sec; seqNo:3385; depth 50.40 2024-11-06T04:58:05Z- handleDVL() - DVL msg epochMillisec: 1730869085.05 2024-11-06T04:58:05Z- handleDvl() - RDI_Pathfinder msg: 1730869085.05 sec; seqNo:1209; #beams:4 2024-11-06T04:58:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.78 , 32.33 , 37.94 , 43.55 2024-11-06T04:58:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.11 , 0.07 2024-11-06T04:58:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869085.06 epoch sec; seqNo:3385; 4061588.18 north; 594861.31 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T04:58:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869081.40 2024-11-06T04:58:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:05Z- waiting for 1730869085.40, time is 1730869085.06 2024-11-06T04:58:05Z- AHRS_M2 msg: 1730869085.41 epoch sec; seqNo:3178 2024-11-06T04:58:05Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.36 psi 2024-11-06T04:58:05Z- handleDepth()- Depth_Keller msg: 1730869085.42 epoch sec; seqNo:3386; depth 50.43 2024-11-06T04:58:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869085.44 epoch sec; seqNo:3386; 4061587.80 north; 594861.31 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:58:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869081.40 2024-11-06T04:58:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:05Z- waiting for 1730869085.40, time is 1730869085.44 2024-11-06T04:58:05Z- LcmTrn::updateTrn() >>>> pose time 1730869085.44, meas time 1730869085.05 <<<< 2024-11-06T04:58:05Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869085.44 2024-11-06T04:58:05Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869085.05 sec, ping # 0; waiting for INS data more recent than 1730869081.40... 2024-11-06T04:58:05Z- TerrainNav::motionUpdate() - currEstimate = (4061588, 594861.31, 50.43) 2024-11-06T04:58:05Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:58:05Z- TerrainNav:: SubcloudNIS is 0.53, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:58:05Z- TNavPF::Projecting Measurements 2024-11-06T04:58:05Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:58:05Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:58:05Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:58:05Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:58:05Z- TNavFilter::4 good beams. 2024-11-06T04:58:05Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:58:05Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:58:05Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:58:05Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:58:05Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:58:05Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:58:05Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:58:05Z- TerrainMapOctree: (vehN, vehE ) = (4061588.13, 594861.43). 2024-11-06T04:58:05Z- TerrainMapOctree: Min Distance = 196.51. 2024-11-06T04:58:05Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:58:05Z- Forcing Subcloud Comparison 2024-11-06T04:58:05Z- Weighting particles with subcloud comparison 2024-11-06T04:58:05Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:05Z- meanExpectedMeasDiff: 3.640319 delta_rms_squared: 1.545396 alpha: 0.987224 2024-11-06T04:58:05Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:05Z- meanExpectedMeasDiff: -4.333392 delta_rms_squared: 1.077265 alpha: 0.981773 2024-11-06T04:58:05Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:05Z- meanExpectedMeasDiff: -2.036558 delta_rms_squared: 1.209930 alpha: 0.983739 2024-11-06T04:58:05Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:05Z- meanExpectedMeasDiff: 1.238053 delta_rms_squared: 1.356434 alpha: 0.985470 2024-11-06T04:58:05Z- Subcloud NIS: 0.561990 2024-11-06T04:58:05Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:58:05Z- TNavPF:: sumWeights = 0.008129 2024-11-06T04:58:05Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:58:05Z- TNavPF::First Term of Map Covariance Matrix is 0.14 2024-11-06T04:58:05Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:58:05Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869085.05 sec. 2024-11-06T04:58:05Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:58:05Z- variances([0], [2], and psi_berg) = 0.703033 0.291037 0.000000 2024-11-06T04:58:05Z- publishEstimatesDecoder() - at 1730869085.44 sec 2024-11-06T04:58:05Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:58:05Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061530.90 594866.65 62.50 2024-11-06T04:58:05Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061531.22 594865.70 61.90 lat/lon: 36.69 -121.94 2024-11-06T04:58:05Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.29 0.69 0.00 2024-11-06T04:58:05Z- AHRS_M2 msg: 1730869085.82 epoch sec; seqNo:3179 2024-11-06T04:58:05Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.36 psi 2024-11-06T04:58:05Z- handleDepth()- Depth_Keller msg: 1730869085.83 epoch sec; seqNo:3387; depth 50.44 2024-11-06T04:58:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869085.85 epoch sec; seqNo:3387; 4061587.80 north; 594861.31 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T04:58:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869085.44 2024-11-06T04:58:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:05Z- waiting for 1730869089.44, time is 1730869085.85 2024-11-06T04:58:06Z- AHRS_M2 msg: 1730869086.22 epoch sec; seqNo:3180 2024-11-06T04:58:06Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.36 psi 2024-11-06T04:58:06Z- handleDepth()- Depth_Keller msg: 1730869086.23 epoch sec; seqNo:3388; depth 50.47 2024-11-06T04:58:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869086.24 epoch sec; seqNo:3388; 4061587.42 north; 594861.32 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T04:58:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869085.44 2024-11-06T04:58:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:06Z- waiting for 1730869089.44, time is 1730869086.24 2024-11-06T04:58:06Z- AHRS_M2 msg: 1730869086.62 epoch sec; seqNo:3181 2024-11-06T04:58:06Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.36 psi 2024-11-06T04:58:06Z- handleDepth()- Depth_Keller msg: 1730869086.63 epoch sec; seqNo:3389; depth 50.47 2024-11-06T04:58:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869086.64 epoch sec; seqNo:3389; 4061587.04 north; 594861.32 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T04:58:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869085.44 2024-11-06T04:58:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:06Z- waiting for 1730869089.44, time is 1730869086.64 2024-11-06T04:58:07Z- AHRS_M2 msg: 1730869087.03 epoch sec; seqNo:3182 2024-11-06T04:58:07Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.36 psi 2024-11-06T04:58:07Z- handleDepth()- Depth_Keller msg: 1730869087.03 epoch sec; seqNo:3390; depth 50.50 2024-11-06T04:58:07Z- handleDVL() - DVL msg epochMillisec: 1730869087.06 2024-11-06T04:58:07Z- handleDvl() - RDI_Pathfinder msg: 1730869087.06 sec; seqNo:1210; #beams:4 2024-11-06T04:58:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.78 , 32.71 , 37.38 , 42.99 2024-11-06T04:58:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.07 , 0.06 2024-11-06T04:58:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869087.07 epoch sec; seqNo:3390; 4061586.66 north; 594861.33 east; 36.70 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T04:58:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869085.44 2024-11-06T04:58:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:07Z- waiting for 1730869089.44, time is 1730869087.07 2024-11-06T04:58:07Z- AHRS_M2 msg: 1730869087.44 epoch sec; seqNo:3183 2024-11-06T04:58:07Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.36 psi 2024-11-06T04:58:07Z- handleDepth()- Depth_Keller msg: 1730869087.44 epoch sec; seqNo:3391; depth 50.52 2024-11-06T04:58:07Z- handleDVL() - DVL msg epochMillisec: 1730869087.44 2024-11-06T04:58:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:58:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:58:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:58:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:58:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:58:07Z- handleDvl() - RDI_Pathfinder msg: 1730869087.44 sec; seqNo:1211; #beams:0 2024-11-06T04:58:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:58:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.07 , 0.06 2024-11-06T04:58:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869087.48 epoch sec; seqNo:3391; 4061586.29 north; 594861.33 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T04:58:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869085.44 2024-11-06T04:58:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:07Z- waiting for 1730869089.44, time is 1730869087.48 2024-11-06T04:58:07Z- AHRS_M2 msg: 1730869087.84 epoch sec; seqNo:3184 2024-11-06T04:58:07Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.36 psi 2024-11-06T04:58:07Z- handleDepth()- Depth_Keller msg: 1730869087.85 epoch sec; seqNo:3392; depth 50.54 2024-11-06T04:58:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869087.86 epoch sec; seqNo:3392; 4061585.91 north; 594861.33 east; 36.70 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T04:58:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869085.44 2024-11-06T04:58:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:07Z- waiting for 1730869089.44, time is 1730869087.86 2024-11-06T04:58:08Z- AHRS_M2 msg: 1730869088.24 epoch sec; seqNo:3185 2024-11-06T04:58:08Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.36 psi 2024-11-06T04:58:08Z- handleDepth()- Depth_Keller msg: 1730869088.25 epoch sec; seqNo:3393; depth 50.55 2024-11-06T04:58:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869088.26 epoch sec; seqNo:3393; 4061585.53 north; 594861.34 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:58:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869085.44 2024-11-06T04:58:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:08Z- waiting for 1730869089.44, time is 1730869088.26 2024-11-06T04:58:08Z- AHRS_M2 msg: 1730869088.64 epoch sec; seqNo:3186 2024-11-06T04:58:08Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.36 psi 2024-11-06T04:58:08Z- handleDepth()- Depth_Keller msg: 1730869088.65 epoch sec; seqNo:3394; depth 50.59 2024-11-06T04:58:08Z- handleDVL() - DVL msg epochMillisec: 1730869088.65 2024-11-06T04:58:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:58:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:58:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:58:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:58:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:58:08Z- handleDvl() - RDI_Pathfinder msg: 1730869088.65 sec; seqNo:1212; #beams:0 2024-11-06T04:58:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:58:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.07 , 0.06 2024-11-06T04:58:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869088.68 epoch sec; seqNo:3394; 4061585.15 north; 594861.34 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T04:58:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869085.44 2024-11-06T04:58:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:08Z- waiting for 1730869089.44, time is 1730869088.68 2024-11-06T04:58:09Z- AHRS_M2 msg: 1730869089.05 epoch sec; seqNo:3187 2024-11-06T04:58:09Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.36 psi 2024-11-06T04:58:09Z- handleDepth()- Depth_Keller msg: 1730869089.05 epoch sec; seqNo:3395; depth 50.60 2024-11-06T04:58:09Z- handleDVL() - DVL msg epochMillisec: 1730869089.07 2024-11-06T04:58:09Z- handleDvl() - RDI_Pathfinder msg: 1730869089.07 sec; seqNo:1213; #beams:4 2024-11-06T04:58:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.41 , 32.52 , 37.57 , 42.42 2024-11-06T04:58:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.02 , 0.04 2024-11-06T04:58:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869089.10 epoch sec; seqNo:3395; 4061584.77 north; 594861.35 east; 36.70 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T04:58:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869085.44 2024-11-06T04:58:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:09Z- waiting for 1730869089.44, time is 1730869089.10 2024-11-06T04:58:09Z- handleDepth()- Depth_Keller msg: 1730869089.47 epoch sec; seqNo:3396; depth 50.61 2024-11-06T04:58:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869089.49 epoch sec; seqNo:3396; 4061584.38 north; 594860.13 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T04:58:09Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:58:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869085.44 2024-11-06T04:58:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:09Z- waiting for 1730869089.44, time is 1730869089.49 2024-11-06T04:58:09Z- LcmTrn::updateTrn() >>>> pose time 1730869089.49, meas time 1730869089.07 <<<< 2024-11-06T04:58:09Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869089.49 2024-11-06T04:58:09Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869089.07 sec, ping # 0; waiting for INS data more recent than 1730869085.44... 2024-11-06T04:58:09Z- TerrainNav::motionUpdate() - currEstimate = (4061584, 594860.13, 50.61) 2024-11-06T04:58:09Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:58:09Z- TerrainNav:: SubcloudNIS is 0.56, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:58:09Z- TNavPF::Projecting Measurements 2024-11-06T04:58:09Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:58:09Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:58:09Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:58:09Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:58:09Z- TNavFilter::4 good beams. 2024-11-06T04:58:09Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:58:09Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:58:09Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:58:09Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:58:09Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:58:09Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:58:09Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:58:09Z- TerrainMapOctree: (vehN, vehE ) = (4061584.73, 594860.25). 2024-11-06T04:58:09Z- TerrainMapOctree: Min Distance = 198.67. 2024-11-06T04:58:09Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:58:09Z- Forcing Subcloud Comparison 2024-11-06T04:58:09Z- Weighting particles with subcloud comparison 2024-11-06T04:58:09Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:09Z- meanExpectedMeasDiff: 4.046600 delta_rms_squared: 1.514260 alpha: 0.986964 2024-11-06T04:58:09Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:09Z- meanExpectedMeasDiff: -3.625745 delta_rms_squared: 1.063144 alpha: 0.981535 2024-11-06T04:58:09Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:09Z- meanExpectedMeasDiff: -1.878913 delta_rms_squared: 1.195230 alpha: 0.983542 2024-11-06T04:58:09Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:09Z- meanExpectedMeasDiff: 0.858903 delta_rms_squared: 1.328339 alpha: 0.985167 2024-11-06T04:58:09Z- Subcloud NIS: 0.588043 2024-11-06T04:58:09Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:58:09Z- TNavPF:: sumWeights = 0.014855 2024-11-06T04:58:09Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:58:09Z- TNavPF::First Term of Map Covariance Matrix is 0.28 2024-11-06T04:58:09Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:58:09Z- TerrainNav::Resampling particle filter... 2024-11-06T04:58:09Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869089.07 sec. 2024-11-06T04:58:09Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:58:09Z- variances([0], [2], and psi_berg) = 0.703689 0.289061 0.000000 2024-11-06T04:58:09Z- publishEstimatesDecoder() - at 1730869089.49 sec 2024-11-06T04:58:09Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:58:09Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061529.75 594864.44 61.86 2024-11-06T04:58:09Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061527.80 594864.52 62.28 lat/lon: 36.69 -121.94 2024-11-06T04:58:09Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.29 0.67 0.00 2024-11-06T04:58:09Z- AHRS_M2 msg: 1730869089.86 epoch sec; seqNo:3188 2024-11-06T04:58:09Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.36 psi 2024-11-06T04:58:09Z- handleDepth()- Depth_Keller msg: 1730869089.87 epoch sec; seqNo:3397; depth 50.62 2024-11-06T04:58:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869089.89 epoch sec; seqNo:3397; 4061584.00 north; 594860.13 east; 36.70 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T04:58:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869089.49 2024-11-06T04:58:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:10Z- waiting for 1730869093.49, time is 1730869089.89 2024-11-06T04:58:10Z- AHRS_M2 msg: 1730869090.26 epoch sec; seqNo:3189 2024-11-06T04:58:10Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.36 psi 2024-11-06T04:58:10Z- handleDepth()- Depth_Keller msg: 1730869090.27 epoch sec; seqNo:3398; depth 50.63 2024-11-06T04:58:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869090.27 epoch sec; seqNo:3398; 4061583.62 north; 594860.14 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T04:58:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869089.49 2024-11-06T04:58:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:10Z- waiting for 1730869093.49, time is 1730869090.27 2024-11-06T04:58:10Z- AHRS_M2 msg: 1730869090.67 epoch sec; seqNo:3190 2024-11-06T04:58:10Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.36 psi 2024-11-06T04:58:10Z- handleDepth()- Depth_Keller msg: 1730869090.67 epoch sec; seqNo:3399; depth 50.64 2024-11-06T04:58:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869090.69 epoch sec; seqNo:3399; 4061583.24 north; 594860.14 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T04:58:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869089.49 2024-11-06T04:58:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:10Z- waiting for 1730869093.49, time is 1730869090.69 2024-11-06T04:58:11Z- AHRS_M2 msg: 1730869091.07 epoch sec; seqNo:3191 2024-11-06T04:58:11Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.35 psi 2024-11-06T04:58:11Z- handleDepth()- Depth_Keller msg: 1730869091.08 epoch sec; seqNo:3400; depth 50.65 2024-11-06T04:58:11Z- handleDVL() - DVL msg epochMillisec: 1730869091.10 2024-11-06T04:58:11Z- handleDvl() - RDI_Pathfinder msg: 1730869091.10 sec; seqNo:1214; #beams:4 2024-11-06T04:58:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.41 , 31.77 , 37.75 , 42.05 2024-11-06T04:58:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.00 2024-11-06T04:58:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869091.13 epoch sec; seqNo:3400; 4061582.86 north; 594860.15 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T04:58:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869089.49 2024-11-06T04:58:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:11Z- waiting for 1730869093.49, time is 1730869091.13 2024-11-06T04:58:11Z- AHRS_M2 msg: 1730869091.47 epoch sec; seqNo:3192 2024-11-06T04:58:11Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.35 psi 2024-11-06T04:58:11Z- handleDepth()- Depth_Keller msg: 1730869091.48 epoch sec; seqNo:3401; depth 50.66 2024-11-06T04:58:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869091.50 epoch sec; seqNo:3401; 4061582.86 north; 594860.15 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T04:58:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869089.49 2024-11-06T04:58:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:11Z- waiting for 1730869093.49, time is 1730869091.50 2024-11-06T04:58:11Z- handleDepth()- Depth_Keller msg: 1730869091.90 epoch sec; seqNo:3402; depth 50.66 2024-11-06T04:58:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869091.91 epoch sec; seqNo:3402; 4061582.48 north; 594860.15 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T04:58:12Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:58:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869089.49 2024-11-06T04:58:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:12Z- waiting for 1730869093.49, time is 1730869091.91 2024-11-06T04:58:12Z- AHRS_M2 msg: 1730869092.28 epoch sec; seqNo:3193 2024-11-06T04:58:12Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.35 psi 2024-11-06T04:58:12Z- handleDepth()- Depth_Keller msg: 1730869092.28 epoch sec; seqNo:3403; depth 50.66 2024-11-06T04:58:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869092.30 epoch sec; seqNo:3403; 4061582.10 north; 594860.16 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T04:58:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869089.49 2024-11-06T04:58:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:12Z- waiting for 1730869093.49, time is 1730869092.30 2024-11-06T04:58:12Z- AHRS_M2 msg: 1730869092.70 epoch sec; seqNo:3194 2024-11-06T04:58:12Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.35 psi 2024-11-06T04:58:12Z- handleDepth()- Depth_Keller msg: 1730869092.72 epoch sec; seqNo:3404; depth 50.67 2024-11-06T04:58:12Z- handleDVL() - DVL msg epochMillisec: 1730869092.77 2024-11-06T04:58:12Z- handleDvl() - RDI_Pathfinder msg: 1730869092.77 sec; seqNo:1215; #beams:4 2024-11-06T04:58:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.41 , 31.40 , 37.94 , 41.30 2024-11-06T04:58:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.03 , -0.02 2024-11-06T04:58:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869092.79 epoch sec; seqNo:3404; 4061581.73 north; 594860.16 east; 36.70 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T04:58:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869089.49 2024-11-06T04:58:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:12Z- waiting for 1730869093.49, time is 1730869092.79 2024-11-06T04:58:13Z- AHRS_M2 msg: 1730869093.09 epoch sec; seqNo:3195 2024-11-06T04:58:13Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.35 psi 2024-11-06T04:58:13Z- handleDepth()- Depth_Keller msg: 1730869093.10 epoch sec; seqNo:3405; depth 50.66 2024-11-06T04:58:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869093.11 epoch sec; seqNo:3405; 4061581.35 north; 594860.16 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T04:58:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869089.49 2024-11-06T04:58:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:13Z- waiting for 1730869093.49, time is 1730869093.11 2024-11-06T04:58:13Z- AHRS_M2 msg: 1730869093.49 epoch sec; seqNo:3196 2024-11-06T04:58:13Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.35 psi 2024-11-06T04:58:13Z- handleDepth()- Depth_Keller msg: 1730869093.50 epoch sec; seqNo:3406; depth 50.67 2024-11-06T04:58:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869093.52 epoch sec; seqNo:3406; 4061580.97 north; 594860.17 east; 36.70 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T04:58:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869089.49 2024-11-06T04:58:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:13Z- waiting for 1730869093.49, time is 1730869093.52 2024-11-06T04:58:13Z- LcmTrn::updateTrn() >>>> pose time 1730869093.52, meas time 1730869092.77 <<<< 2024-11-06T04:58:13Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869093.52 2024-11-06T04:58:13Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869092.77 sec, ping # 0; waiting for INS data more recent than 1730869089.49... 2024-11-06T04:58:13Z- TerrainNav::motionUpdate() - currEstimate = (4061581, 594860.17, 50.67) 2024-11-06T04:58:13Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:58:13Z- TerrainNav:: SubcloudNIS is 0.59, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:58:13Z- TNavPF::Projecting Measurements 2024-11-06T04:58:13Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:58:13Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:58:13Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:58:13Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:58:13Z- TNavFilter::4 good beams. 2024-11-06T04:58:13Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:58:13Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:58:13Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:58:13Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:58:13Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:58:13Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:58:13Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:58:13Z- TerrainMapOctree: (vehN, vehE ) = (4061581.60, 594860.16). 2024-11-06T04:58:13Z- TerrainMapOctree: Min Distance = 201.23. 2024-11-06T04:58:13Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:58:13Z- Forcing Subcloud Comparison 2024-11-06T04:58:13Z- Weighting particles with subcloud comparison 2024-11-06T04:58:13Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:13Z- meanExpectedMeasDiff: 4.228196 delta_rms_squared: 1.514546 alpha: 0.986967 2024-11-06T04:58:13Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:13Z- meanExpectedMeasDiff: -4.358653 delta_rms_squared: 1.049573 alpha: 0.981301 2024-11-06T04:58:13Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:13Z- meanExpectedMeasDiff: -1.547003 delta_rms_squared: 1.207982 alpha: 0.983713 2024-11-06T04:58:13Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:13Z- meanExpectedMeasDiff: 0.425862 delta_rms_squared: 1.295313 alpha: 0.984794 2024-11-06T04:58:13Z- Subcloud NIS: 0.620675 2024-11-06T04:58:13Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:58:13Z- TNavPF:: sumWeights = 0.006642 2024-11-06T04:58:13Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:58:13Z- TNavPF::First Term of Map Covariance Matrix is 0.15 2024-11-06T04:58:13Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:58:13Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869092.77 sec. 2024-11-06T04:58:13Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:58:13Z- variances([0], [2], and psi_berg) = 0.700511 0.291615 0.000000 2024-11-06T04:58:13Z- publishEstimatesDecoder() - at 1730869093.52 sec 2024-11-06T04:58:13Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:58:13Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061523.12 594864.50 62.93 2024-11-06T04:58:13Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061524.39 594864.55 62.66 lat/lon: 36.69 -121.94 2024-11-06T04:58:13Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.70 0.29 0.67 0.00 2024-11-06T04:58:13Z- AHRS_M2 msg: 1730869093.89 epoch sec; seqNo:3197 2024-11-06T04:58:13Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.35 psi 2024-11-06T04:58:13Z- handleDepth()- Depth_Keller msg: 1730869093.90 epoch sec; seqNo:3407; depth 50.66 2024-11-06T04:58:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869093.92 epoch sec; seqNo:3407; 4061580.59 north; 594860.17 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T04:58:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869093.52 2024-11-06T04:58:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:14Z- waiting for 1730869097.52, time is 1730869093.92 2024-11-06T04:58:14Z- AHRS_M2 msg: 1730869094.30 epoch sec; seqNo:3198 2024-11-06T04:58:14Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.35 psi 2024-11-06T04:58:14Z- handleDepth()- Depth_Keller msg: 1730869094.31 epoch sec; seqNo:3408; depth 50.67 2024-11-06T04:58:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869094.32 epoch sec; seqNo:3408; 4061580.21 north; 594860.18 east; 36.70 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T04:58:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869093.52 2024-11-06T04:58:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:14Z- waiting for 1730869097.52, time is 1730869094.32 2024-11-06T04:58:14Z- AHRS_M2 msg: 1730869094.74 epoch sec; seqNo:3199 2024-11-06T04:58:14Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.35 psi 2024-11-06T04:58:14Z- handleDepth()- Depth_Keller msg: 1730869094.75 epoch sec; seqNo:3409; depth 50.66 2024-11-06T04:58:14Z- handleDVL() - DVL msg epochMillisec: 1730869094.77 2024-11-06T04:58:14Z- handleDvl() - RDI_Pathfinder msg: 1730869094.77 sec; seqNo:1216; #beams:4 2024-11-06T04:58:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.59 , 31.21 , 38.31 , 41.12 2024-11-06T04:58:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.08 , -0.02 2024-11-06T04:58:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869094.79 epoch sec; seqNo:3409; 4061579.83 north; 594860.18 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T04:58:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869093.52 2024-11-06T04:58:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:14Z- waiting for 1730869097.52, time is 1730869094.79 2024-11-06T04:58:15Z- AHRS_M2 msg: 1730869095.11 epoch sec; seqNo:3200 2024-11-06T04:58:15Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.35 psi 2024-11-06T04:58:15Z- handleDepth()- Depth_Keller msg: 1730869095.11 epoch sec; seqNo:3410; depth 50.65 2024-11-06T04:58:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869095.14 epoch sec; seqNo:3410; 4061579.44 north; 594858.96 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T04:58:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869093.52 2024-11-06T04:58:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:15Z- waiting for 1730869097.52, time is 1730869095.14 2024-11-06T04:58:15Z- AHRS_M2 msg: 1730869095.51 epoch sec; seqNo:3201 2024-11-06T04:58:15Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.35 psi 2024-11-06T04:58:15Z- handleDepth()- Depth_Keller msg: 1730869095.52 epoch sec; seqNo:3411; depth 50.66 2024-11-06T04:58:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869095.54 epoch sec; seqNo:3411; 4061579.06 north; 594858.97 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T04:58:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869093.52 2024-11-06T04:58:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:15Z- waiting for 1730869097.52, time is 1730869095.54 2024-11-06T04:58:15Z- AHRS_M2 msg: 1730869095.92 epoch sec; seqNo:3202 2024-11-06T04:58:15Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.35 psi 2024-11-06T04:58:15Z- handleDepth()- Depth_Keller msg: 1730869095.92 epoch sec; seqNo:3412; depth 50.65 2024-11-06T04:58:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869095.93 epoch sec; seqNo:3412; 4061578.68 north; 594858.97 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T04:58:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869093.52 2024-11-06T04:58:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:16Z- waiting for 1730869097.52, time is 1730869095.93 2024-11-06T04:58:16Z- AHRS_M2 msg: 1730869096.32 epoch sec; seqNo:3203 2024-11-06T04:58:16Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.35 psi 2024-11-06T04:58:16Z- handleDepth()- Depth_Keller msg: 1730869096.32 epoch sec; seqNo:3413; depth 50.65 2024-11-06T04:58:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869096.34 epoch sec; seqNo:3413; 4061578.30 north; 594858.98 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T04:58:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869093.52 2024-11-06T04:58:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:16Z- waiting for 1730869097.52, time is 1730869096.34 2024-11-06T04:58:16Z- AHRS_M2 msg: 1730869096.75 epoch sec; seqNo:3204 2024-11-06T04:58:16Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.35 psi 2024-11-06T04:58:16Z- handleDepth()- Depth_Keller msg: 1730869096.76 epoch sec; seqNo:3414; depth 50.64 2024-11-06T04:58:16Z- handleDVL() - DVL msg epochMillisec: 1730869096.77 2024-11-06T04:58:16Z- handleDvl() - RDI_Pathfinder msg: 1730869096.77 sec; seqNo:1217; #beams:4 2024-11-06T04:58:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.78 , 31.21 , 38.69 , 40.37 2024-11-06T04:58:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.12 , -0.01 2024-11-06T04:58:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869096.79 epoch sec; seqNo:3414; 4061577.92 north; 594858.98 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T04:58:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869093.52 2024-11-06T04:58:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:16Z- waiting for 1730869097.52, time is 1730869096.79 2024-11-06T04:58:17Z- AHRS_M2 msg: 1730869097.13 epoch sec; seqNo:3205 2024-11-06T04:58:17Z- AHRS_M2 msg: 0.17 phi; -0.01 theta; 3.35 psi 2024-11-06T04:58:17Z- handleDepth()- Depth_Keller msg: 1730869097.13 epoch sec; seqNo:3415; depth 50.64 2024-11-06T04:58:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869097.15 epoch sec; seqNo:3415; 4061577.92 north; 594858.98 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T04:58:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869093.52 2024-11-06T04:58:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:17Z- waiting for 1730869097.52, time is 1730869097.15 2024-11-06T04:58:17Z- AHRS_M2 msg: 1730869097.54 epoch sec; seqNo:3206 2024-11-06T04:58:17Z- AHRS_M2 msg: 0.17 phi; -0.01 theta; 3.35 psi 2024-11-06T04:58:17Z- handleDepth()- Depth_Keller msg: 1730869097.54 epoch sec; seqNo:3416; depth 50.63 2024-11-06T04:58:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869097.55 epoch sec; seqNo:3416; 4061577.55 north; 594858.99 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T04:58:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869093.52 2024-11-06T04:58:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:17Z- waiting for 1730869097.52, time is 1730869097.55 2024-11-06T04:58:17Z- LcmTrn::updateTrn() >>>> pose time 1730869097.55, meas time 1730869096.77 <<<< 2024-11-06T04:58:17Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869097.55 2024-11-06T04:58:17Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869096.77 sec, ping # 0; waiting for INS data more recent than 1730869093.52... 2024-11-06T04:58:17Z- TerrainNav::motionUpdate() - currEstimate = (4061578, 594858.99, 50.63) 2024-11-06T04:58:17Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:58:17Z- TerrainNav:: SubcloudNIS is 0.62, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:58:17Z- TNavPF::Projecting Measurements 2024-11-06T04:58:17Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:58:17Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:58:17Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:58:17Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:58:17Z- TNavFilter::4 good beams. 2024-11-06T04:58:17Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:58:17Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:58:17Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:58:17Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:58:17Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:58:17Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:58:17Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:58:17Z- TerrainMapOctree: (vehN, vehE ) = (4061578.21, 594859.21). 2024-11-06T04:58:17Z- TerrainMapOctree: Min Distance = 203.57. 2024-11-06T04:58:17Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:58:17Z- Forcing Subcloud Comparison 2024-11-06T04:58:17Z- Weighting particles with subcloud comparison 2024-11-06T04:58:17Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:17Z- meanExpectedMeasDiff: 4.623019 delta_rms_squared: 1.538771 alpha: 0.987169 2024-11-06T04:58:17Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:17Z- meanExpectedMeasDiff: -4.063279 delta_rms_squared: 1.039112 alpha: 0.981116 2024-11-06T04:58:17Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:17Z- meanExpectedMeasDiff: -0.911157 delta_rms_squared: 1.238989 alpha: 0.984114 2024-11-06T04:58:17Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:17Z- meanExpectedMeasDiff: 0.234338 delta_rms_squared: 1.269527 alpha: 0.984490 2024-11-06T04:58:17Z- Subcloud NIS: 0.649255 2024-11-06T04:58:17Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:58:17Z- TNavPF:: sumWeights = 0.008056 2024-11-06T04:58:17Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:58:17Z- TNavPF::First Term of Map Covariance Matrix is 0.19 2024-11-06T04:58:17Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:58:17Z- TerrainNav::Resampling particle filter... 2024-11-06T04:58:17Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869096.77 sec. 2024-11-06T04:58:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:58:17Z- variances([0], [2], and psi_berg) = 0.708354 0.296047 0.000000 2024-11-06T04:58:17Z- publishEstimatesDecoder() - at 1730869097.55 sec 2024-11-06T04:58:17Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:58:17Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061522.90 594863.37 63.33 2024-11-06T04:58:17Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061520.97 594863.37 62.80 lat/lon: 36.69 -121.94 2024-11-06T04:58:17Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.71 0.30 0.66 0.00 2024-11-06T04:58:17Z- AHRS_M2 msg: 1730869097.94 epoch sec; seqNo:3207 2024-11-06T04:58:17Z- AHRS_M2 msg: 0.17 phi; -0.01 theta; 3.35 psi 2024-11-06T04:58:17Z- handleDepth()- Depth_Keller msg: 1730869097.95 epoch sec; seqNo:3417; depth 50.63 2024-11-06T04:58:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869097.96 epoch sec; seqNo:3417; 4061577.17 north; 594858.99 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T04:58:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869097.55 2024-11-06T04:58:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:18Z- waiting for 1730869101.55, time is 1730869097.96 2024-11-06T04:58:18Z- AHRS_M2 msg: 1730869098.34 epoch sec; seqNo:3208 2024-11-06T04:58:18Z- AHRS_M2 msg: 0.17 phi; -0.00 theta; 3.35 psi 2024-11-06T04:58:18Z- handleDepth()- Depth_Keller msg: 1730869098.34 epoch sec; seqNo:3418; depth 50.65 2024-11-06T04:58:18Z- handleDVL() - DVL msg epochMillisec: 1730869098.35 2024-11-06T04:58:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:58:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:58:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:58:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:58:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:58:18Z- handleDvl() - RDI_Pathfinder msg: 1730869098.35 sec; seqNo:1218; #beams:0 2024-11-06T04:58:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:58:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.12 , -0.01 2024-11-06T04:58:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869098.35 epoch sec; seqNo:3418; 4061576.79 north; 594858.99 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T04:58:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869097.55 2024-11-06T04:58:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:18Z- waiting for 1730869101.55, time is 1730869098.35 2024-11-06T04:58:18Z- AHRS_M2 msg: 1730869098.76 epoch sec; seqNo:3209 2024-11-06T04:58:18Z- AHRS_M2 msg: 0.17 phi; -0.00 theta; 3.35 psi 2024-11-06T04:58:18Z- handleDepth()- Depth_Keller msg: 1730869098.76 epoch sec; seqNo:3419; depth 50.65 2024-11-06T04:58:18Z- handleDVL() - DVL msg epochMillisec: 1730869098.78 2024-11-06T04:58:18Z- handleDvl() - RDI_Pathfinder msg: 1730869098.78 sec; seqNo:1219; #beams:4 2024-11-06T04:58:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.34 , 31.02 , 40.00 , 39.62 2024-11-06T04:58:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.14 , 0.03 2024-11-06T04:58:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869098.81 epoch sec; seqNo:3419; 4061576.41 north; 594859.00 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T04:58:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869097.55 2024-11-06T04:58:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:18Z- waiting for 1730869101.55, time is 1730869098.81 2024-11-06T04:58:19Z- AHRS_M2 msg: 1730869099.15 epoch sec; seqNo:3210 2024-11-06T04:58:19Z- AHRS_M2 msg: 0.17 phi; -0.00 theta; 3.35 psi 2024-11-06T04:58:19Z- handleDepth()- Depth_Keller msg: 1730869099.16 epoch sec; seqNo:3420; depth 50.63 2024-11-06T04:58:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869099.18 epoch sec; seqNo:3420; 4061576.03 north; 594859.00 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T04:58:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869097.55 2024-11-06T04:58:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:19Z- waiting for 1730869101.55, time is 1730869099.18 2024-11-06T04:58:19Z- AHRS_M2 msg: 1730869099.55 epoch sec; seqNo:3211 2024-11-06T04:58:19Z- AHRS_M2 msg: 0.17 phi; 0.00 theta; 3.35 psi 2024-11-06T04:58:19Z- handleDepth()- Depth_Keller msg: 1730869099.56 epoch sec; seqNo:3421; depth 50.65 2024-11-06T04:58:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869099.57 epoch sec; seqNo:3421; 4061575.64 north; 594857.79 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T04:58:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869097.55 2024-11-06T04:58:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:19Z- waiting for 1730869101.55, time is 1730869099.57 2024-11-06T04:58:19Z- AHRS_M2 msg: 1730869099.96 epoch sec; seqNo:3212 2024-11-06T04:58:19Z- AHRS_M2 msg: 0.17 phi; 0.00 theta; 3.35 psi 2024-11-06T04:58:19Z- handleDepth()- Depth_Keller msg: 1730869099.96 epoch sec; seqNo:3422; depth 50.65 2024-11-06T04:58:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869099.97 epoch sec; seqNo:3422; 4061575.64 north; 594857.79 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T04:58:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869097.55 2024-11-06T04:58:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:20Z- waiting for 1730869101.55, time is 1730869099.97 2024-11-06T04:58:20Z- AHRS_M2 msg: 1730869100.36 epoch sec; seqNo:3213 2024-11-06T04:58:20Z- AHRS_M2 msg: 0.17 phi; 0.00 theta; 3.35 psi 2024-11-06T04:58:20Z- handleDepth()- Depth_Keller msg: 1730869100.38 epoch sec; seqNo:3423; depth 50.66 2024-11-06T04:58:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869100.38 epoch sec; seqNo:3423; 4061575.26 north; 594857.79 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T04:58:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869097.55 2024-11-06T04:58:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:20Z- waiting for 1730869101.55, time is 1730869100.38 2024-11-06T04:58:20Z- AHRS_M2 msg: 1730869100.76 epoch sec; seqNo:3214 2024-11-06T04:58:20Z- AHRS_M2 msg: 0.17 phi; 0.00 theta; 3.35 psi 2024-11-06T04:58:20Z- handleDepth()- Depth_Keller msg: 1730869100.77 epoch sec; seqNo:3424; depth 50.67 2024-11-06T04:58:20Z- handleDVL() - DVL msg epochMillisec: 1730869100.81 2024-11-06T04:58:20Z- handleDvl() - RDI_Pathfinder msg: 1730869100.81 sec; seqNo:1220; #beams:4 2024-11-06T04:58:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.15 , 30.84 , 41.12 , 39.06 2024-11-06T04:58:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.12 , 0.07 2024-11-06T04:58:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869100.83 epoch sec; seqNo:3424; 4061574.88 north; 594857.79 east; 36.70 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T04:58:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869097.55 2024-11-06T04:58:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:20Z- waiting for 1730869101.55, time is 1730869100.83 2024-11-06T04:58:21Z- AHRS_M2 msg: 1730869101.17 epoch sec; seqNo:3215 2024-11-06T04:58:21Z- AHRS_M2 msg: 0.17 phi; 0.01 theta; 3.35 psi 2024-11-06T04:58:21Z- handleDepth()- Depth_Keller msg: 1730869101.17 epoch sec; seqNo:3425; depth 50.67 2024-11-06T04:58:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869101.22 epoch sec; seqNo:3425; 4061574.50 north; 594857.80 east; 36.70 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T04:58:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869097.55 2024-11-06T04:58:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:21Z- waiting for 1730869101.55, time is 1730869101.22 2024-11-06T04:58:21Z- AHRS_M2 msg: 1730869101.57 epoch sec; seqNo:3216 2024-11-06T04:58:21Z- AHRS_M2 msg: 0.17 phi; 0.01 theta; 3.35 psi 2024-11-06T04:58:21Z- handleDepth()- Depth_Keller msg: 1730869101.59 epoch sec; seqNo:3426; depth 50.68 2024-11-06T04:58:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869101.60 epoch sec; seqNo:3426; 4061574.12 north; 594857.80 east; 36.70 lat; -121.94 lon; 50.68 depth; utmZone: 10 2024-11-06T04:58:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869097.55 2024-11-06T04:58:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:21Z- waiting for 1730869101.55, time is 1730869101.60 2024-11-06T04:58:21Z- LcmTrn::updateTrn() >>>> pose time 1730869101.60, meas time 1730869100.81 <<<< 2024-11-06T04:58:21Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869101.60 2024-11-06T04:58:21Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869100.81 sec, ping # 0; waiting for INS data more recent than 1730869097.55... 2024-11-06T04:58:21Z- TerrainNav::motionUpdate() - currEstimate = (4061574, 594857.80, 50.68) 2024-11-06T04:58:21Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:58:21Z- TerrainNav:: SubcloudNIS is 0.65, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:58:21Z- TNavPF::Projecting Measurements 2024-11-06T04:58:21Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:58:21Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:58:21Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:58:21Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:58:21Z- TNavFilter::4 good beams. 2024-11-06T04:58:21Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:58:21Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:58:21Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:58:21Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:58:21Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:58:21Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:58:21Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:58:21Z- TerrainMapOctree: (vehN, vehE ) = (4061574.79, 594858.03). 2024-11-06T04:58:21Z- TerrainMapOctree: Min Distance = 205.84. 2024-11-06T04:58:21Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:58:21Z- Forcing Subcloud Comparison 2024-11-06T04:58:21Z- Weighting particles with subcloud comparison 2024-11-06T04:58:21Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:21Z- meanExpectedMeasDiff: 5.147482 delta_rms_squared: 1.519002 alpha: 0.987005 2024-11-06T04:58:21Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:21Z- meanExpectedMeasDiff: -4.175533 delta_rms_squared: 1.017772 alpha: 0.980728 2024-11-06T04:58:21Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:21Z- meanExpectedMeasDiff: 2.496238 delta_rms_squared: 1.169455 alpha: 0.983186 2024-11-06T04:58:21Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:21Z- meanExpectedMeasDiff: -0.486411 delta_rms_squared: 1.224838 alpha: 0.983934 2024-11-06T04:58:21Z- Subcloud NIS: 0.688595 2024-11-06T04:58:21Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:58:21Z- TNavPF:: sumWeights = 0.002381 2024-11-06T04:58:21Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:58:21Z- TNavPF::First Term of Map Covariance Matrix is 0.06 2024-11-06T04:58:21Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:58:21Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869100.81 sec. 2024-11-06T04:58:21Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:58:21Z- variances([0], [2], and psi_berg) = 0.728701 0.299904 0.000000 2024-11-06T04:58:21Z- publishEstimatesDecoder() - at 1730869101.60 sec 2024-11-06T04:58:21Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:58:21Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061518.55 594863.14 63.28 2024-11-06T04:58:21Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061517.57 594862.20 62.71 lat/lon: 36.69 -121.94 2024-11-06T04:58:21Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.73 0.30 0.68 0.00 2024-11-06T04:58:21Z- AHRS_M2 msg: 1730869101.97 epoch sec; seqNo:3217 2024-11-06T04:58:21Z- AHRS_M2 msg: 0.17 phi; 0.01 theta; 3.35 psi 2024-11-06T04:58:22Z- handleDepth()- Depth_Keller msg: 1730869102.00 epoch sec; seqNo:3427; depth 50.66 2024-11-06T04:58:22Z- handleDVL() - DVL msg epochMillisec: 1730869102.00 2024-11-06T04:58:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:58:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:58:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:58:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:58:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:58:22Z- handleDvl() - RDI_Pathfinder msg: 1730869102.00 sec; seqNo:1221; #beams:0 2024-11-06T04:58:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:58:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.12 , 0.07 2024-11-06T04:58:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869102.02 epoch sec; seqNo:3427; 4061573.74 north; 594857.81 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T04:58:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869101.60 2024-11-06T04:58:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:22Z- waiting for 1730869105.60, time is 1730869102.02 2024-11-06T04:58:22Z- AHRS_M2 msg: 1730869102.38 epoch sec; seqNo:3218 2024-11-06T04:58:22Z- AHRS_M2 msg: 0.17 phi; 0.01 theta; 3.35 psi 2024-11-06T04:58:22Z- handleDepth()- Depth_Keller msg: 1730869102.38 epoch sec; seqNo:3428; depth 50.69 2024-11-06T04:58:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869102.39 epoch sec; seqNo:3428; 4061573.36 north; 594857.81 east; 36.70 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T04:58:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869101.60 2024-11-06T04:58:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:22Z- waiting for 1730869105.60, time is 1730869102.39 2024-11-06T04:58:22Z- AHRS_M2 msg: 1730869102.78 epoch sec; seqNo:3219 2024-11-06T04:58:22Z- AHRS_M2 msg: 0.17 phi; 0.01 theta; 3.35 psi 2024-11-06T04:58:22Z- handleDepth()- Depth_Keller msg: 1730869102.79 epoch sec; seqNo:3429; depth 50.68 2024-11-06T04:58:22Z- handleDVL() - DVL msg epochMillisec: 1730869102.82 2024-11-06T04:58:22Z- handleDvl() - RDI_Pathfinder msg: 1730869102.82 sec; seqNo:1222; #beams:4 2024-11-06T04:58:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.15 , 30.65 , 41.49 , 38.69 2024-11-06T04:58:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , 0.08 2024-11-06T04:58:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869102.83 epoch sec; seqNo:3429; 4061572.99 north; 594857.82 east; 36.70 lat; -121.94 lon; 50.68 depth; utmZone: 10 2024-11-06T04:58:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869101.60 2024-11-06T04:58:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:22Z- waiting for 1730869105.60, time is 1730869102.83 2024-11-06T04:58:23Z- AHRS_M2 msg: 1730869103.19 epoch sec; seqNo:3220 2024-11-06T04:58:23Z- AHRS_M2 msg: 0.17 phi; 0.01 theta; 3.35 psi 2024-11-06T04:58:23Z- handleDepth()- Depth_Keller msg: 1730869103.19 epoch sec; seqNo:3430; depth 50.67 2024-11-06T04:58:23Z- handleDVL() - DVL msg epochMillisec: 1730869103.19 2024-11-06T04:58:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:58:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:58:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:58:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:58:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:58:23Z- handleDvl() - RDI_Pathfinder msg: 1730869103.19 sec; seqNo:1223; #beams:0 2024-11-06T04:58:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:58:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , 0.08 2024-11-06T04:58:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869103.21 epoch sec; seqNo:3430; 4061572.61 north; 594857.82 east; 36.70 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T04:58:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869101.60 2024-11-06T04:58:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:23Z- waiting for 1730869105.60, time is 1730869103.21 2024-11-06T04:58:23Z- AHRS_M2 msg: 1730869103.59 epoch sec; seqNo:3221 2024-11-06T04:58:23Z- AHRS_M2 msg: 0.17 phi; 0.01 theta; 3.35 psi 2024-11-06T04:58:23Z- handleDepth()- Depth_Keller msg: 1730869103.60 epoch sec; seqNo:3431; depth 50.68 2024-11-06T04:58:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869103.61 epoch sec; seqNo:3431; 4061572.23 north; 594857.82 east; 36.70 lat; -121.94 lon; 50.68 depth; utmZone: 10 2024-11-06T04:58:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869101.60 2024-11-06T04:58:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:23Z- waiting for 1730869105.60, time is 1730869103.61 2024-11-06T04:58:24Z- handleDepth()- Depth_Keller msg: 1730869104.02 epoch sec; seqNo:3432; depth 50.66 2024-11-06T04:58:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869104.03 epoch sec; seqNo:3432; 4061571.84 north; 594856.61 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T04:58:24Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:58:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869101.60 2024-11-06T04:58:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:24Z- waiting for 1730869105.60, time is 1730869104.03 2024-11-06T04:58:24Z- AHRS_M2 msg: 1730869104.40 epoch sec; seqNo:3222 2024-11-06T04:58:24Z- AHRS_M2 msg: 0.17 phi; 0.01 theta; 3.35 psi 2024-11-06T04:58:24Z- handleDepth()- Depth_Keller msg: 1730869104.40 epoch sec; seqNo:3433; depth 50.66 2024-11-06T04:58:24Z- handleDVL() - DVL msg epochMillisec: 1730869104.41 2024-11-06T04:58:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:58:24Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:58:24Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:58:24Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:58:24Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:58:24Z- handleDvl() - RDI_Pathfinder msg: 1730869104.41 sec; seqNo:1224; #beams:0 2024-11-06T04:58:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:58:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , 0.08 2024-11-06T04:58:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869104.42 epoch sec; seqNo:3433; 4061571.84 north; 594856.61 east; 36.70 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T04:58:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869101.60 2024-11-06T04:58:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:24Z- waiting for 1730869105.60, time is 1730869104.42 2024-11-06T04:58:24Z- AHRS_M2 msg: 1730869104.81 epoch sec; seqNo:3223 2024-11-06T04:58:24Z- AHRS_M2 msg: 0.17 phi; 0.01 theta; 3.35 psi 2024-11-06T04:58:24Z- handleDepth()- Depth_Keller msg: 1730869104.82 epoch sec; seqNo:3434; depth 50.65 2024-11-06T04:58:24Z- handleDVL() - DVL msg epochMillisec: 1730869104.85 2024-11-06T04:58:24Z- handleDvl() - RDI_Pathfinder msg: 1730869104.85 sec; seqNo:1225; #beams:4 2024-11-06T04:58:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.53 , 30.84 , 42.05 , 37.94 2024-11-06T04:58:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.01 , 0.04 2024-11-06T04:58:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869104.86 epoch sec; seqNo:3434; 4061571.46 north; 594856.61 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T04:58:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869101.60 2024-11-06T04:58:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:24Z- waiting for 1730869105.60, time is 1730869104.86 2024-11-06T04:58:25Z- AHRS_M2 msg: 1730869105.21 epoch sec; seqNo:3224 2024-11-06T04:58:25Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 3.35 psi 2024-11-06T04:58:25Z- handleDepth()- Depth_Keller msg: 1730869105.21 epoch sec; seqNo:3435; depth 50.65 2024-11-06T04:58:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869105.23 epoch sec; seqNo:3435; 4061571.08 north; 594856.62 east; 36.70 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T04:58:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869101.60 2024-11-06T04:58:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:25Z- waiting for 1730869105.60, time is 1730869105.23 2024-11-06T04:58:25Z- AHRS_M2 msg: 1730869105.61 epoch sec; seqNo:3225 2024-11-06T04:58:25Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:25Z- handleDepth()- Depth_Keller msg: 1730869105.63 epoch sec; seqNo:3436; depth 50.64 2024-11-06T04:58:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869105.64 epoch sec; seqNo:3436; 4061570.70 north; 594856.62 east; 36.70 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T04:58:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869101.60 2024-11-06T04:58:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:25Z- waiting for 1730869105.60, time is 1730869105.64 2024-11-06T04:58:25Z- LcmTrn::updateTrn() >>>> pose time 1730869105.64, meas time 1730869104.85 <<<< 2024-11-06T04:58:25Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869105.64 2024-11-06T04:58:25Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869104.85 sec, ping # 0; waiting for INS data more recent than 1730869101.60... 2024-11-06T04:58:25Z- TerrainNav::motionUpdate() - currEstimate = (4061571, 594856.62, 50.64) 2024-11-06T04:58:25Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:58:25Z- TerrainNav:: SubcloudNIS is 0.69, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:58:25Z- TNavPF::Projecting Measurements 2024-11-06T04:58:25Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:58:25Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:58:25Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:58:25Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:58:25Z- TNavFilter::4 good beams. 2024-11-06T04:58:25Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:58:25Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:58:25Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:58:25Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:58:25Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:58:25Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:58:25Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:58:25Z- TerrainMapOctree: (vehN, vehE ) = (4061571.37, 594856.85). 2024-11-06T04:58:25Z- TerrainMapOctree: Min Distance = 208.15. 2024-11-06T04:58:25Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:58:25Z- Forcing Subcloud Comparison 2024-11-06T04:58:25Z- Weighting particles with subcloud comparison 2024-11-06T04:58:25Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:25Z- meanExpectedMeasDiff: 5.350209 delta_rms_squared: 1.540470 alpha: 0.987183 2024-11-06T04:58:25Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:25Z- meanExpectedMeasDiff: -4.360247 delta_rms_squared: 1.034830 alpha: 0.981040 2024-11-06T04:58:25Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:25Z- meanExpectedMeasDiff: 3.064719 delta_rms_squared: 1.271886 alpha: 0.984519 2024-11-06T04:58:25Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:25Z- meanExpectedMeasDiff: -1.560353 delta_rms_squared: 1.229587 alpha: 0.983995 2024-11-06T04:58:25Z- Subcloud NIS: 0.741103 2024-11-06T04:58:25Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:58:25Z- TNavPF:: sumWeights = 0.000864 2024-11-06T04:58:25Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:58:25Z- TNavPF::First Term of Map Covariance Matrix is 0.02 2024-11-06T04:58:25Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:58:25Z- TerrainNav::Resampling particle filter... 2024-11-06T04:58:25Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869104.85 sec. 2024-11-06T04:58:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:58:25Z- variances([0], [2], and psi_berg) = 0.734751 0.298642 0.000000 2024-11-06T04:58:25Z- publishEstimatesDecoder() - at 1730869105.64 sec 2024-11-06T04:58:25Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:58:25Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061516.16 594860.97 61.99 2024-11-06T04:58:25Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061514.14 594861.01 62.60 lat/lon: 36.69 -121.94 2024-11-06T04:58:25Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.73 0.30 0.66 0.00 2024-11-06T04:58:26Z- AHRS_M2 msg: 1730869106.02 epoch sec; seqNo:3226 2024-11-06T04:58:26Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:26Z- handleDepth()- Depth_Keller msg: 1730869106.03 epoch sec; seqNo:3437; depth 50.63 2024-11-06T04:58:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869106.05 epoch sec; seqNo:3437; 4061570.32 north; 594856.62 east; 36.70 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T04:58:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869105.64 2024-11-06T04:58:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:26Z- waiting for 1730869109.64, time is 1730869106.05 2024-11-06T04:58:26Z- AHRS_M2 msg: 1730869106.43 epoch sec; seqNo:3227 2024-11-06T04:58:26Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:26Z- handleDepth()- Depth_Keller msg: 1730869106.43 epoch sec; seqNo:3438; depth 50.61 2024-11-06T04:58:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869106.44 epoch sec; seqNo:3438; 4061569.94 north; 594856.63 east; 36.70 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T04:58:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869105.64 2024-11-06T04:58:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:26Z- waiting for 1730869109.64, time is 1730869106.44 2024-11-06T04:58:26Z- AHRS_M2 msg: 1730869106.83 epoch sec; seqNo:3228 2024-11-06T04:58:26Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:26Z- handleDepth()- Depth_Keller msg: 1730869106.84 epoch sec; seqNo:3439; depth 50.59 2024-11-06T04:58:26Z- handleDVL() - DVL msg epochMillisec: 1730869106.85 2024-11-06T04:58:26Z- handleDvl() - RDI_Pathfinder msg: 1730869106.85 sec; seqNo:1226; #beams:4 2024-11-06T04:58:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.53 , 30.84 , 42.42 , 38.13 2024-11-06T04:58:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.01 , -0.03 2024-11-06T04:58:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869106.87 epoch sec; seqNo:3439; 4061569.56 north; 594856.63 east; 36.70 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T04:58:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869105.64 2024-11-06T04:58:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:26Z- waiting for 1730869109.64, time is 1730869106.87 2024-11-06T04:58:27Z- AHRS_M2 msg: 1730869107.23 epoch sec; seqNo:3229 2024-11-06T04:58:27Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:27Z- handleDepth()- Depth_Keller msg: 1730869107.23 epoch sec; seqNo:3440; depth 50.56 2024-11-06T04:58:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869107.26 epoch sec; seqNo:3440; 4061569.18 north; 594856.64 east; 36.70 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T04:58:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869105.64 2024-11-06T04:58:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:27Z- waiting for 1730869109.64, time is 1730869107.26 2024-11-06T04:58:27Z- AHRS_M2 msg: 1730869107.63 epoch sec; seqNo:3230 2024-11-06T04:58:27Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:27Z- handleDepth()- Depth_Keller msg: 1730869107.64 epoch sec; seqNo:3441; depth 50.55 2024-11-06T04:58:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869107.65 epoch sec; seqNo:3441; 4061568.80 north; 594856.64 east; 36.70 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:58:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869105.64 2024-11-06T04:58:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:27Z- waiting for 1730869109.64, time is 1730869107.65 2024-11-06T04:58:28Z- AHRS_M2 msg: 1730869108.04 epoch sec; seqNo:3231 2024-11-06T04:58:28Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:28Z- handleDepth()- Depth_Keller msg: 1730869108.04 epoch sec; seqNo:3442; depth 50.53 2024-11-06T04:58:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869108.05 epoch sec; seqNo:3442; 4061568.43 north; 594856.65 east; 36.70 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T04:58:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869105.64 2024-11-06T04:58:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:28Z- waiting for 1730869109.64, time is 1730869108.05 2024-11-06T04:58:28Z- AHRS_M2 msg: 1730869108.44 epoch sec; seqNo:3232 2024-11-06T04:58:28Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:28Z- handleDepth()- Depth_Keller msg: 1730869108.44 epoch sec; seqNo:3443; depth 50.52 2024-11-06T04:58:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869108.46 epoch sec; seqNo:3443; 4061568.05 north; 594856.65 east; 36.70 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T04:58:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869105.64 2024-11-06T04:58:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:28Z- waiting for 1730869109.64, time is 1730869108.46 2024-11-06T04:58:28Z- AHRS_M2 msg: 1730869108.84 epoch sec; seqNo:3233 2024-11-06T04:58:28Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:28Z- handleDepth()- Depth_Keller msg: 1730869108.85 epoch sec; seqNo:3444; depth 50.49 2024-11-06T04:58:28Z- handleDVL() - DVL msg epochMillisec: 1730869108.88 2024-11-06T04:58:28Z- handleDvl() - RDI_Pathfinder msg: 1730869108.88 sec; seqNo:1227; #beams:4 2024-11-06T04:58:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.34 , 31.02 , 42.42 , 37.75 2024-11-06T04:58:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , -0.06 2024-11-06T04:58:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869108.89 epoch sec; seqNo:3444; 4061567.67 north; 594856.65 east; 36.70 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T04:58:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869105.64 2024-11-06T04:58:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:29Z- waiting for 1730869109.64, time is 1730869108.89 2024-11-06T04:58:29Z- AHRS_M2 msg: 1730869109.25 epoch sec; seqNo:3234 2024-11-06T04:58:29Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:29Z- handleDepth()- Depth_Keller msg: 1730869109.26 epoch sec; seqNo:3445; depth 50.47 2024-11-06T04:58:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869109.28 epoch sec; seqNo:3445; 4061567.29 north; 594856.66 east; 36.70 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T04:58:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869105.64 2024-11-06T04:58:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:29Z- waiting for 1730869109.64, time is 1730869109.28 2024-11-06T04:58:29Z- AHRS_M2 msg: 1730869109.66 epoch sec; seqNo:3235 2024-11-06T04:58:29Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:29Z- handleDepth()- Depth_Keller msg: 1730869109.67 epoch sec; seqNo:3446; depth 50.44 2024-11-06T04:58:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869109.68 epoch sec; seqNo:3446; 4061566.91 north; 594856.66 east; 36.70 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T04:58:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869105.64 2024-11-06T04:58:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:29Z- waiting for 1730869109.64, time is 1730869109.68 2024-11-06T04:58:29Z- LcmTrn::updateTrn() >>>> pose time 1730869109.68, meas time 1730869108.88 <<<< 2024-11-06T04:58:29Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869109.68 2024-11-06T04:58:29Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869108.88 sec, ping # 0; waiting for INS data more recent than 1730869105.64... 2024-11-06T04:58:29Z- TerrainNav::motionUpdate() - currEstimate = (4061567, 594856.66, 50.44) 2024-11-06T04:58:29Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:58:29Z- TerrainNav:: SubcloudNIS is 0.74, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:58:29Z- TNavPF::Projecting Measurements 2024-11-06T04:58:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:58:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:58:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:58:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:58:29Z- TNavFilter::4 good beams. 2024-11-06T04:58:29Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:58:29Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:58:29Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:58:29Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:58:29Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:58:29Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:58:29Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:58:29Z- TerrainMapOctree: (vehN, vehE ) = (4061567.66, 594856.65). 2024-11-06T04:58:29Z- TerrainMapOctree: Min Distance = 211.24. 2024-11-06T04:58:29Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:58:29Z- Forcing Subcloud Comparison 2024-11-06T04:58:29Z- Weighting particles with subcloud comparison 2024-11-06T04:58:29Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:29Z- meanExpectedMeasDiff: 4.872189 delta_rms_squared: 1.530522 alpha: 0.987101 2024-11-06T04:58:29Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:29Z- meanExpectedMeasDiff: -4.444363 delta_rms_squared: 1.029935 alpha: 0.980951 2024-11-06T04:58:29Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:29Z- meanExpectedMeasDiff: 3.017720 delta_rms_squared: 1.273667 alpha: 0.984540 2024-11-06T04:58:29Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:29Z- meanExpectedMeasDiff: -1.893792 delta_rms_squared: 1.214540 alpha: 0.983800 2024-11-06T04:58:29Z- Subcloud NIS: 0.791844 2024-11-06T04:58:29Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:58:29Z- TNavPF:: sumWeights = 0.001073 2024-11-06T04:58:29Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:58:29Z- TNavPF::First Term of Map Covariance Matrix is 0.03 2024-11-06T04:58:29Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:58:29Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869108.88 sec. 2024-11-06T04:58:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:58:29Z- variances([0], [2], and psi_berg) = 0.733187 0.298255 0.000000 2024-11-06T04:58:29Z- publishEstimatesDecoder() - at 1730869109.68 sec 2024-11-06T04:58:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:58:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061508.39 594860.83 62.23 2024-11-06T04:58:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061510.35 594861.05 62.44 lat/lon: 36.69 -121.94 2024-11-06T04:58:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.73 0.30 0.67 0.00 2024-11-06T04:58:30Z- AHRS_M2 msg: 1730869110.06 epoch sec; seqNo:3236 2024-11-06T04:58:30Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:30Z- handleDepth()- Depth_Keller msg: 1730869110.07 epoch sec; seqNo:3447; depth 50.43 2024-11-06T04:58:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869110.08 epoch sec; seqNo:3447; 4061566.52 north; 594855.45 east; 36.70 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:58:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869109.68 2024-11-06T04:58:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:30Z- waiting for 1730869113.68, time is 1730869110.08 2024-11-06T04:58:30Z- AHRS_M2 msg: 1730869110.46 epoch sec; seqNo:3237 2024-11-06T04:58:30Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:30Z- handleDepth()- Depth_Keller msg: 1730869110.47 epoch sec; seqNo:3448; depth 50.40 2024-11-06T04:58:30Z- handleDVL() - DVL msg epochMillisec: 1730869110.47 2024-11-06T04:58:30Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:30Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:30Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:30Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:58:30Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:58:30Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:58:30Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:58:30Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:58:30Z- handleDvl() - RDI_Pathfinder msg: 1730869110.47 sec; seqNo:1228; #beams:0 2024-11-06T04:58:30Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:58:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.04 , -0.06 2024-11-06T04:58:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869110.48 epoch sec; seqNo:3448; 4061566.14 north; 594855.45 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T04:58:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869109.68 2024-11-06T04:58:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:30Z- waiting for 1730869113.68, time is 1730869110.48 2024-11-06T04:58:30Z- AHRS_M2 msg: 1730869110.87 epoch sec; seqNo:3238 2024-11-06T04:58:30Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:30Z- handleDepth()- Depth_Keller msg: 1730869110.88 epoch sec; seqNo:3449; depth 50.40 2024-11-06T04:58:30Z- handleDVL() - DVL msg epochMillisec: 1730869110.90 2024-11-06T04:58:30Z- handleDvl() - RDI_Pathfinder msg: 1730869110.90 sec; seqNo:1229; #beams:4 2024-11-06T04:58:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.71 , 30.46 , 42.42 , 37.75 2024-11-06T04:58:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.14 , -0.03 2024-11-06T04:58:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869110.94 epoch sec; seqNo:3449; 4061565.76 north; 594855.45 east; 36.70 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T04:58:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869109.68 2024-11-06T04:58:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:31Z- waiting for 1730869113.68, time is 1730869110.94 2024-11-06T04:58:31Z- AHRS_M2 msg: 1730869111.27 epoch sec; seqNo:3239 2024-11-06T04:58:31Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:31Z- handleDepth()- Depth_Keller msg: 1730869111.27 epoch sec; seqNo:3450; depth 50.37 2024-11-06T04:58:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869111.30 epoch sec; seqNo:3450; 4061565.76 north; 594855.45 east; 36.70 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:58:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869109.68 2024-11-06T04:58:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:31Z- waiting for 1730869113.68, time is 1730869111.30 2024-11-06T04:58:31Z- AHRS_M2 msg: 1730869111.69 epoch sec; seqNo:3240 2024-11-06T04:58:31Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:31Z- handleDepth()- Depth_Keller msg: 1730869111.71 epoch sec; seqNo:3451; depth 50.35 2024-11-06T04:58:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869111.72 epoch sec; seqNo:3451; 4061565.38 north; 594855.46 east; 36.70 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:58:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869109.68 2024-11-06T04:58:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:31Z- waiting for 1730869113.68, time is 1730869111.72 2024-11-06T04:58:32Z- AHRS_M2 msg: 1730869112.08 epoch sec; seqNo:3241 2024-11-06T04:58:32Z- AHRS_M2 msg: 0.17 phi; 0.03 theta; 3.36 psi 2024-11-06T04:58:32Z- handleDepth()- Depth_Keller msg: 1730869112.09 epoch sec; seqNo:3452; depth 50.32 2024-11-06T04:58:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869112.10 epoch sec; seqNo:3452; 4061565.00 north; 594855.46 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:58:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869109.68 2024-11-06T04:58:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:32Z- waiting for 1730869113.68, time is 1730869112.10 2024-11-06T04:58:32Z- AHRS_M2 msg: 1730869112.48 epoch sec; seqNo:3242 2024-11-06T04:58:32Z- AHRS_M2 msg: 0.17 phi; 0.03 theta; 3.35 psi 2024-11-06T04:58:32Z- handleDepth()- Depth_Keller msg: 1730869112.48 epoch sec; seqNo:3453; depth 50.32 2024-11-06T04:58:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869112.49 epoch sec; seqNo:3453; 4061564.62 north; 594855.47 east; 36.70 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:58:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869109.68 2024-11-06T04:58:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:32Z- waiting for 1730869113.68, time is 1730869112.49 2024-11-06T04:58:32Z- AHRS_M2 msg: 1730869112.89 epoch sec; seqNo:3243 2024-11-06T04:58:32Z- AHRS_M2 msg: 0.17 phi; 0.03 theta; 3.35 psi 2024-11-06T04:58:32Z- handleDepth()- Depth_Keller msg: 1730869112.90 epoch sec; seqNo:3454; depth 50.30 2024-11-06T04:58:32Z- handleDVL() - DVL msg epochMillisec: 1730869112.92 2024-11-06T04:58:32Z- handleDvl() - RDI_Pathfinder msg: 1730869112.92 sec; seqNo:1230; #beams:4 2024-11-06T04:58:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.53 , 30.46 , 42.61 , 37.38 2024-11-06T04:58:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , 0.16 , 0.04 2024-11-06T04:58:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869112.94 epoch sec; seqNo:3454; 4061564.24 north; 594855.47 east; 36.70 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:58:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869109.68 2024-11-06T04:58:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:33Z- waiting for 1730869113.68, time is 1730869112.94 2024-11-06T04:58:33Z- AHRS_M2 msg: 1730869113.29 epoch sec; seqNo:3244 2024-11-06T04:58:33Z- AHRS_M2 msg: 0.17 phi; 0.03 theta; 3.35 psi 2024-11-06T04:58:33Z- handleDepth()- Depth_Keller msg: 1730869113.29 epoch sec; seqNo:3455; depth 50.29 2024-11-06T04:58:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869113.31 epoch sec; seqNo:3455; 4061563.87 north; 594855.48 east; 36.70 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:58:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869109.68 2024-11-06T04:58:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:33Z- waiting for 1730869113.68, time is 1730869113.31 2024-11-06T04:58:33Z- AHRS_M2 msg: 1730869113.69 epoch sec; seqNo:3245 2024-11-06T04:58:33Z- AHRS_M2 msg: 0.17 phi; 0.03 theta; 3.35 psi 2024-11-06T04:58:33Z- handleDepth()- Depth_Keller msg: 1730869113.70 epoch sec; seqNo:3456; depth 50.27 2024-11-06T04:58:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869113.71 epoch sec; seqNo:3456; 4061563.87 north; 594855.48 east; 36.69 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T04:58:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869109.68 2024-11-06T04:58:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:33Z- waiting for 1730869113.68, time is 1730869113.71 2024-11-06T04:58:33Z- LcmTrn::updateTrn() >>>> pose time 1730869113.71, meas time 1730869112.92 <<<< 2024-11-06T04:58:33Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869113.71 2024-11-06T04:58:33Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869112.92 sec, ping # 0; waiting for INS data more recent than 1730869109.68... 2024-11-06T04:58:33Z- TerrainNav::motionUpdate() - currEstimate = (4061564, 594855.48, 50.27) 2024-11-06T04:58:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:58:33Z- TerrainNav:: SubcloudNIS is 0.79, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:58:33Z- TNavPF::Projecting Measurements 2024-11-06T04:58:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:58:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:58:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:58:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:58:33Z- TNavFilter::4 good beams. 2024-11-06T04:58:33Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:58:33Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:58:33Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:58:33Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:58:33Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:58:33Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:58:33Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:58:33Z- TerrainMapOctree: (vehN, vehE ) = (4061564.46, 594855.71). 2024-11-06T04:58:33Z- TerrainMapOctree: Min Distance = 213.54. 2024-11-06T04:58:33Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:58:33Z- Forcing Subcloud Comparison 2024-11-06T04:58:33Z- Weighting particles with subcloud comparison 2024-11-06T04:58:33Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:33Z- meanExpectedMeasDiff: 4.938768 delta_rms_squared: 1.522491 alpha: 0.987034 2024-11-06T04:58:33Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:33Z- meanExpectedMeasDiff: -5.160196 delta_rms_squared: 0.995559 alpha: 0.980306 2024-11-06T04:58:33Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:33Z- meanExpectedMeasDiff: 2.937093 delta_rms_squared: 1.277135 alpha: 0.984581 2024-11-06T04:58:33Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:33Z- meanExpectedMeasDiff: -2.317850 delta_rms_squared: 1.204702 alpha: 0.983669 2024-11-06T04:58:34Z- Subcloud NIS: 0.847097 2024-11-06T04:58:34Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:58:34Z- TNavPF:: sumWeights = 0.000306 2024-11-06T04:58:34Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:58:34Z- TNavPF::First Term of Map Covariance Matrix is 0.01 2024-11-06T04:58:34Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:58:34Z- TerrainNav::Resampling particle filter... 2024-11-06T04:58:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869112.92 sec. 2024-11-06T04:58:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:58:34Z- variances([0], [2], and psi_berg) = 0.758576 0.355183 0.000000 2024-11-06T04:58:34Z- publishEstimatesDecoder() - at 1730869113.71 sec 2024-11-06T04:58:34Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:58:34Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061509.25 594859.90 62.81 2024-11-06T04:58:34Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061507.27 594859.81 62.44 lat/lon: 36.69 -121.94 2024-11-06T04:58:34Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.76 0.36 0.69 0.00 2024-11-06T04:58:34Z- AHRS_M2 msg: 1730869114.10 epoch sec; seqNo:3246 2024-11-06T04:58:34Z- AHRS_M2 msg: 0.17 phi; 0.03 theta; 3.35 psi 2024-11-06T04:58:34Z- handleDepth()- Depth_Keller msg: 1730869114.12 epoch sec; seqNo:3457; depth 50.27 2024-11-06T04:58:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869114.15 epoch sec; seqNo:3457; 4061563.47 north; 594854.26 east; 36.69 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T04:58:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869113.71 2024-11-06T04:58:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:34Z- waiting for 1730869117.71, time is 1730869114.15 2024-11-06T04:58:34Z- AHRS_M2 msg: 1730869114.50 epoch sec; seqNo:3247 2024-11-06T04:58:34Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 3.35 psi 2024-11-06T04:58:34Z- handleDepth()- Depth_Keller msg: 1730869114.50 epoch sec; seqNo:3458; depth 50.26 2024-11-06T04:58:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869114.51 epoch sec; seqNo:3458; 4061563.09 north; 594854.26 east; 36.69 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:58:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869113.71 2024-11-06T04:58:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:34Z- waiting for 1730869117.71, time is 1730869114.51 2024-11-06T04:58:34Z- AHRS_M2 msg: 1730869114.90 epoch sec; seqNo:3248 2024-11-06T04:58:34Z- AHRS_M2 msg: 0.17 phi; 0.02 theta; 3.35 psi 2024-11-06T04:58:34Z- handleDepth()- Depth_Keller msg: 1730869114.92 epoch sec; seqNo:3459; depth 50.27 2024-11-06T04:58:34Z- handleDVL() - DVL msg epochMillisec: 1730869114.95 2024-11-06T04:58:34Z- handleDvl() - RDI_Pathfinder msg: 1730869114.95 sec; seqNo:1231; #beams:4 2024-11-06T04:58:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.34 , 30.84 , 42.05 , 37.57 2024-11-06T04:58:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.12 , 0.08 2024-11-06T04:58:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869114.96 epoch sec; seqNo:3459; 4061562.72 north; 594854.27 east; 36.69 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T04:58:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869113.71 2024-11-06T04:58:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:35Z- waiting for 1730869117.71, time is 1730869114.96 2024-11-06T04:58:35Z- AHRS_M2 msg: 1730869115.31 epoch sec; seqNo:3249 2024-11-06T04:58:35Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 3.35 psi 2024-11-06T04:58:35Z- handleDepth()- Depth_Keller msg: 1730869115.32 epoch sec; seqNo:3460; depth 50.25 2024-11-06T04:58:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869115.34 epoch sec; seqNo:3460; 4061562.34 north; 594854.27 east; 36.69 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T04:58:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869113.71 2024-11-06T04:58:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:35Z- waiting for 1730869117.71, time is 1730869115.34 2024-11-06T04:58:35Z- AHRS_M2 msg: 1730869115.71 epoch sec; seqNo:3250 2024-11-06T04:58:35Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 3.35 psi 2024-11-06T04:58:35Z- handleDepth()- Depth_Keller msg: 1730869115.72 epoch sec; seqNo:3461; depth 50.24 2024-11-06T04:58:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869115.73 epoch sec; seqNo:3461; 4061561.96 north; 594854.28 east; 36.69 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:58:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869113.71 2024-11-06T04:58:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:35Z- waiting for 1730869117.71, time is 1730869115.73 2024-11-06T04:58:36Z- AHRS_M2 msg: 1730869116.12 epoch sec; seqNo:3251 2024-11-06T04:58:36Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:36Z- handleDepth()- Depth_Keller msg: 1730869116.12 epoch sec; seqNo:3462; depth 50.24 2024-11-06T04:58:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869116.13 epoch sec; seqNo:3462; 4061561.58 north; 594854.28 east; 36.69 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T04:58:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869113.71 2024-11-06T04:58:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:36Z- waiting for 1730869117.71, time is 1730869116.13 2024-11-06T04:58:36Z- AHRS_M2 msg: 1730869116.52 epoch sec; seqNo:3252 2024-11-06T04:58:36Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:36Z- handleDepth()- Depth_Keller msg: 1730869116.53 epoch sec; seqNo:3463; depth 50.23 2024-11-06T04:58:36Z- handleDVL() - DVL msg epochMillisec: 1730869116.53 2024-11-06T04:58:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:58:36Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:58:36Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:58:36Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:58:36Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:58:36Z- handleDvl() - RDI_Pathfinder msg: 1730869116.53 sec; seqNo:1232; #beams:0 2024-11-06T04:58:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:58:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.12 , 0.08 2024-11-06T04:58:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869116.54 epoch sec; seqNo:3463; 4061561.20 north; 594854.28 east; 36.69 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T04:58:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869113.71 2024-11-06T04:58:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:36Z- waiting for 1730869117.71, time is 1730869116.54 2024-11-06T04:58:36Z- AHRS_M2 msg: 1730869116.92 epoch sec; seqNo:3253 2024-11-06T04:58:36Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:36Z- handleDepth()- Depth_Keller msg: 1730869116.93 epoch sec; seqNo:3464; depth 50.23 2024-11-06T04:58:36Z- handleDVL() - DVL msg epochMillisec: 1730869116.96 2024-11-06T04:58:36Z- handleDvl() - RDI_Pathfinder msg: 1730869116.96 sec; seqNo:1233; #beams:4 2024-11-06T04:58:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.34 , 31.02 , 41.87 , 37.57 2024-11-06T04:58:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.05 , 0.08 2024-11-06T04:58:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869116.98 epoch sec; seqNo:3464; 4061560.82 north; 594854.29 east; 36.69 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T04:58:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869113.71 2024-11-06T04:58:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:37Z- waiting for 1730869117.71, time is 1730869116.98 2024-11-06T04:58:37Z- AHRS_M2 msg: 1730869117.33 epoch sec; seqNo:3254 2024-11-06T04:58:37Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:37Z- handleDepth()- Depth_Keller msg: 1730869117.33 epoch sec; seqNo:3465; depth 50.20 2024-11-06T04:58:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869117.36 epoch sec; seqNo:3465; 4061560.82 north; 594854.29 east; 36.69 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:58:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869113.71 2024-11-06T04:58:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:37Z- waiting for 1730869117.71, time is 1730869117.36 2024-11-06T04:58:37Z- AHRS_M2 msg: 1730869117.73 epoch sec; seqNo:3255 2024-11-06T04:58:37Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:37Z- handleDepth()- Depth_Keller msg: 1730869117.74 epoch sec; seqNo:3466; depth 50.20 2024-11-06T04:58:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869117.75 epoch sec; seqNo:3466; 4061560.44 north; 594854.29 east; 36.69 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:58:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869113.71 2024-11-06T04:58:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:37Z- waiting for 1730869117.71, time is 1730869117.75 2024-11-06T04:58:37Z- LcmTrn::updateTrn() >>>> pose time 1730869117.75, meas time 1730869116.96 <<<< 2024-11-06T04:58:37Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869117.75 2024-11-06T04:58:37Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869116.96 sec, ping # 0; waiting for INS data more recent than 1730869113.71... 2024-11-06T04:58:37Z- TerrainNav::motionUpdate() - currEstimate = (4061560, 594854.29, 50.20) 2024-11-06T04:58:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:58:37Z- TerrainNav:: SubcloudNIS is 0.85, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:58:37Z- TNavPF::Projecting Measurements 2024-11-06T04:58:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:58:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:58:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:58:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:58:37Z- TNavFilter::4 good beams. 2024-11-06T04:58:37Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:58:37Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:58:37Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:58:37Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:58:37Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:58:37Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:58:37Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:58:37Z- TerrainMapOctree: (vehN, vehE ) = (4061561.11, 594854.52). 2024-11-06T04:58:37Z- TerrainMapOctree: Min Distance = 215.88. 2024-11-06T04:58:37Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:58:38Z- Forcing Subcloud Comparison 2024-11-06T04:58:38Z- Weighting particles with subcloud comparison 2024-11-06T04:58:38Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:38Z- meanExpectedMeasDiff: 5.000939 delta_rms_squared: 1.566679 alpha: 0.987395 2024-11-06T04:58:38Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:38Z- meanExpectedMeasDiff: -3.740316 delta_rms_squared: 0.961598 alpha: 0.979625 2024-11-06T04:58:38Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:38Z- meanExpectedMeasDiff: 2.463209 delta_rms_squared: 1.310955 alpha: 0.984973 2024-11-06T04:58:38Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:38Z- meanExpectedMeasDiff: -2.129330 delta_rms_squared: 1.253658 alpha: 0.984297 2024-11-06T04:58:38Z- Subcloud NIS: 0.892435 2024-11-06T04:58:38Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:58:38Z- TNavPF:: sumWeights = 0.003003 2024-11-06T04:58:38Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:58:38Z- TNavPF::First Term of Map Covariance Matrix is 0.07 2024-11-06T04:58:38Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:58:38Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869116.96 sec. 2024-11-06T04:58:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:58:38Z- variances([0], [2], and psi_berg) = 0.776968 0.319388 0.000000 2024-11-06T04:58:38Z- publishEstimatesDecoder() - at 1730869117.75 sec 2024-11-06T04:58:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:58:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061503.48 594858.68 62.92 2024-11-06T04:58:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061503.75 594858.52 62.33 lat/lon: 36.69 -121.94 2024-11-06T04:58:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.78 0.32 0.67 0.00 2024-11-06T04:58:38Z- handleDepth()- Depth_Keller msg: 1730869118.16 epoch sec; seqNo:3467; depth 50.22 2024-11-06T04:58:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869118.17 epoch sec; seqNo:3467; 4061560.05 north; 594853.08 east; 36.69 lat; -121.94 lon; 50.22 depth; utmZone: 10 2024-11-06T04:58:38Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:58:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869117.75 2024-11-06T04:58:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:38Z- waiting for 1730869121.75, time is 1730869118.17 2024-11-06T04:58:38Z- AHRS_M2 msg: 1730869118.54 epoch sec; seqNo:3256 2024-11-06T04:58:38Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 3.36 psi 2024-11-06T04:58:38Z- handleDepth()- Depth_Keller msg: 1730869118.55 epoch sec; seqNo:3468; depth 50.20 2024-11-06T04:58:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869118.56 epoch sec; seqNo:3468; 4061559.67 north; 594853.08 east; 36.69 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:58:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869117.75 2024-11-06T04:58:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:38Z- waiting for 1730869121.75, time is 1730869118.56 2024-11-06T04:58:38Z- handleDepth()- Depth_Keller msg: 1730869118.96 epoch sec; seqNo:3469; depth 50.20 2024-11-06T04:58:39Z- handleDVL() - DVL msg epochMillisec: 1730869118.99 2024-11-06T04:58:39Z- handleDvl() - RDI_Pathfinder msg: 1730869118.99 sec; seqNo:1234; #beams:4 2024-11-06T04:58:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.34 , 31.58 , 41.68 , 37.94 2024-11-06T04:58:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.03 2024-11-06T04:58:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869119.00 epoch sec; seqNo:3469; 4061559.29 north; 594853.08 east; 36.69 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:58:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869117.75 2024-11-06T04:58:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:39Z- waiting for 1730869121.75, time is 1730869119.00 2024-11-06T04:58:39Z- AHRS_M2 msg: 1730869119.35 epoch sec; seqNo:3257 2024-11-06T04:58:39Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 3.37 psi 2024-11-06T04:58:39Z- handleDepth()- Depth_Keller msg: 1730869119.35 epoch sec; seqNo:3470; depth 50.19 2024-11-06T04:58:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869119.37 epoch sec; seqNo:3470; 4061558.91 north; 594853.09 east; 36.69 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:58:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869117.75 2024-11-06T04:58:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:39Z- waiting for 1730869121.75, time is 1730869119.37 2024-11-06T04:58:39Z- AHRS_M2 msg: 1730869119.75 epoch sec; seqNo:3258 2024-11-06T04:58:39Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.37 psi 2024-11-06T04:58:39Z- handleDepth()- Depth_Keller msg: 1730869119.76 epoch sec; seqNo:3471; depth 50.18 2024-11-06T04:58:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869119.77 epoch sec; seqNo:3471; 4061558.54 north; 594853.09 east; 36.69 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T04:58:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869117.75 2024-11-06T04:58:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:39Z- waiting for 1730869121.75, time is 1730869119.77 2024-11-06T04:58:40Z- AHRS_M2 msg: 1730869120.15 epoch sec; seqNo:3259 2024-11-06T04:58:40Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.37 psi 2024-11-06T04:58:40Z- handleDepth()- Depth_Keller msg: 1730869120.17 epoch sec; seqNo:3472; depth 50.19 2024-11-06T04:58:40Z- handleDVL() - DVL msg epochMillisec: 1730869120.17 2024-11-06T04:58:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:58:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:58:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:58:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:58:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:58:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:58:40Z- handleDvl() - RDI_Pathfinder msg: 1730869120.17 sec; seqNo:1235; #beams:0 2024-11-06T04:58:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:58:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.03 2024-11-06T04:58:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869120.18 epoch sec; seqNo:3472; 4061558.16 north; 594853.10 east; 36.69 lat; -121.94 lon; 50.19 depth; utmZone: 10 2024-11-06T04:58:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869117.75 2024-11-06T04:58:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:40Z- waiting for 1730869121.75, time is 1730869120.18 2024-11-06T04:58:40Z- AHRS_M2 msg: 1730869120.56 epoch sec; seqNo:3260 2024-11-06T04:58:40Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.37 psi 2024-11-06T04:58:40Z- handleDepth()- Depth_Keller msg: 1730869120.57 epoch sec; seqNo:3473; depth 50.17 2024-11-06T04:58:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869120.58 epoch sec; seqNo:3473; 4061557.78 north; 594853.10 east; 36.69 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T04:58:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869117.75 2024-11-06T04:58:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:40Z- waiting for 1730869121.75, time is 1730869120.58 2024-11-06T04:58:40Z- AHRS_M2 msg: 1730869120.96 epoch sec; seqNo:3261 2024-11-06T04:58:40Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.37 psi 2024-11-06T04:58:40Z- handleDepth()- Depth_Keller msg: 1730869120.97 epoch sec; seqNo:3474; depth 50.14 2024-11-06T04:58:41Z- handleDVL() - DVL msg epochMillisec: 1730869121.00 2024-11-06T04:58:41Z- handleDvl() - RDI_Pathfinder msg: 1730869121.00 sec; seqNo:1236; #beams:4 2024-11-06T04:58:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 50.09 , 31.58 , 41.30 , 37.75 2024-11-06T04:58:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , -0.02 2024-11-06T04:58:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869121.03 epoch sec; seqNo:3474; 4061557.40 north; 594853.11 east; 36.69 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:58:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869117.75 2024-11-06T04:58:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:41Z- waiting for 1730869121.75, time is 1730869121.03 2024-11-06T04:58:41Z- handleDepth()- Depth_Keller msg: 1730869121.39 epoch sec; seqNo:3475; depth 50.13 2024-11-06T04:58:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869121.40 epoch sec; seqNo:3475; 4061557.02 north; 594853.11 east; 36.69 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T04:58:41Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:58:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869117.75 2024-11-06T04:58:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:41Z- waiting for 1730869121.75, time is 1730869121.40 2024-11-06T04:58:41Z- AHRS_M2 msg: 1730869121.77 epoch sec; seqNo:3262 2024-11-06T04:58:41Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.37 psi 2024-11-06T04:58:41Z- handleDepth()- Depth_Keller msg: 1730869121.78 epoch sec; seqNo:3476; depth 50.12 2024-11-06T04:58:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869121.79 epoch sec; seqNo:3476; 4061556.64 north; 594853.11 east; 36.69 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T04:58:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869117.75 2024-11-06T04:58:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:41Z- waiting for 1730869121.75, time is 1730869121.79 2024-11-06T04:58:41Z- LcmTrn::updateTrn() >>>> pose time 1730869121.79, meas time 1730869121.00 <<<< 2024-11-06T04:58:41Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869121.79 2024-11-06T04:58:41Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869121.00 sec, ping # 0; waiting for INS data more recent than 1730869117.75... 2024-11-06T04:58:41Z- TerrainNav::motionUpdate() - currEstimate = (4061557, 594853.11, 50.12) 2024-11-06T04:58:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:58:41Z- TerrainNav:: SubcloudNIS is 0.89, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:58:41Z- TNavPF::Projecting Measurements 2024-11-06T04:58:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:58:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:58:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:58:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:58:41Z- TNavFilter::4 good beams. 2024-11-06T04:58:41Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:58:41Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:58:41Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:58:41Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:58:41Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:58:41Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:58:41Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:58:41Z- TerrainMapOctree: (vehN, vehE ) = (4061557.39, 594853.35). 2024-11-06T04:58:41Z- TerrainMapOctree: Min Distance = 218.59. 2024-11-06T04:58:41Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:58:42Z- Forcing Subcloud Comparison 2024-11-06T04:58:42Z- Weighting particles with subcloud comparison 2024-11-06T04:58:42Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:42Z- meanExpectedMeasDiff: 6.086288 delta_rms_squared: 1.486869 alpha: 0.986727 2024-11-06T04:58:42Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:42Z- meanExpectedMeasDiff: -3.248156 delta_rms_squared: 1.047389 alpha: 0.981263 2024-11-06T04:58:42Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:42Z- meanExpectedMeasDiff: 1.948018 delta_rms_squared: 1.260221 alpha: 0.984378 2024-11-06T04:58:42Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:42Z- meanExpectedMeasDiff: -2.094769 delta_rms_squared: 1.217588 alpha: 0.983840 2024-11-06T04:58:42Z- Subcloud NIS: 0.919425 2024-11-06T04:58:42Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:58:42Z- TNavPF:: sumWeights = 0.002318 2024-11-06T04:58:42Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:58:42Z- TNavPF::First Term of Map Covariance Matrix is 0.08 2024-11-06T04:58:42Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:58:42Z- TerrainNav::Resampling particle filter... 2024-11-06T04:58:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869121.00 sec. 2024-11-06T04:58:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:58:42Z- variances([0], [2], and psi_berg) = 0.776136 0.318186 0.000000 2024-11-06T04:58:42Z- publishEstimatesDecoder() - at 1730869121.79 sec 2024-11-06T04:58:42Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:58:42Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061502.14 594857.40 63.45 2024-11-06T04:58:42Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061499.95 594857.34 62.16 lat/lon: 36.69 -121.94 2024-11-06T04:58:42Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.78 0.32 0.68 0.00 2024-11-06T04:58:42Z- AHRS_M2 msg: 1730869122.17 epoch sec; seqNo:3263 2024-11-06T04:58:42Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.37 psi 2024-11-06T04:58:42Z- handleDepth()- Depth_Keller msg: 1730869122.19 epoch sec; seqNo:3477; depth 50.10 2024-11-06T04:58:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869122.23 epoch sec; seqNo:3477; 4061556.26 north; 594853.12 east; 36.69 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T04:58:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869121.79 2024-11-06T04:58:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:42Z- waiting for 1730869125.79, time is 1730869122.23 2024-11-06T04:58:42Z- AHRS_M2 msg: 1730869122.58 epoch sec; seqNo:3264 2024-11-06T04:58:42Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.37 psi 2024-11-06T04:58:42Z- handleDepth()- Depth_Keller msg: 1730869122.58 epoch sec; seqNo:3478; depth 50.09 2024-11-06T04:58:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869122.59 epoch sec; seqNo:3478; 4061555.88 north; 594853.12 east; 36.69 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T04:58:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869121.79 2024-11-06T04:58:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:42Z- waiting for 1730869125.79, time is 1730869122.59 2024-11-06T04:58:42Z- AHRS_M2 msg: 1730869122.98 epoch sec; seqNo:3265 2024-11-06T04:58:42Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.37 psi 2024-11-06T04:58:43Z- handleDepth()- Depth_Keller msg: 1730869123.00 epoch sec; seqNo:3479; depth 50.07 2024-11-06T04:58:43Z- handleDVL() - DVL msg epochMillisec: 1730869123.02 2024-11-06T04:58:43Z- handleDvl() - RDI_Pathfinder msg: 1730869123.02 sec; seqNo:1237; #beams:4 2024-11-06T04:58:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.97 , 31.77 , 41.12 , 38.13 2024-11-06T04:58:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.09 , -0.03 2024-11-06T04:58:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869123.03 epoch sec; seqNo:3479; 4061555.50 north; 594853.13 east; 36.69 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T04:58:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869121.79 2024-11-06T04:58:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:43Z- waiting for 1730869125.79, time is 1730869123.03 2024-11-06T04:58:43Z- AHRS_M2 msg: 1730869123.39 epoch sec; seqNo:3266 2024-11-06T04:58:43Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.37 psi 2024-11-06T04:58:43Z- handleDepth()- Depth_Keller msg: 1730869123.39 epoch sec; seqNo:3480; depth 50.06 2024-11-06T04:58:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869123.40 epoch sec; seqNo:3480; 4061555.49 north; 594851.91 east; 36.69 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T04:58:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869121.79 2024-11-06T04:58:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:43Z- waiting for 1730869125.79, time is 1730869123.40 2024-11-06T04:58:43Z- AHRS_M2 msg: 1730869123.80 epoch sec; seqNo:3267 2024-11-06T04:58:43Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.37 psi 2024-11-06T04:58:43Z- handleDepth()- Depth_Keller msg: 1730869123.80 epoch sec; seqNo:3481; depth 50.06 2024-11-06T04:58:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869123.82 epoch sec; seqNo:3481; 4061555.11 north; 594851.91 east; 36.69 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T04:58:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869121.79 2024-11-06T04:58:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:43Z- waiting for 1730869125.79, time is 1730869123.82 2024-11-06T04:58:44Z- AHRS_M2 msg: 1730869124.20 epoch sec; seqNo:3268 2024-11-06T04:58:44Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.37 psi 2024-11-06T04:58:44Z- handleDepth()- Depth_Keller msg: 1730869124.21 epoch sec; seqNo:3482; depth 50.03 2024-11-06T04:58:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869124.22 epoch sec; seqNo:3482; 4061554.73 north; 594851.91 east; 36.69 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T04:58:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869121.79 2024-11-06T04:58:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:44Z- waiting for 1730869125.79, time is 1730869124.22 2024-11-06T04:58:44Z- AHRS_M2 msg: 1730869124.60 epoch sec; seqNo:3269 2024-11-06T04:58:44Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.37 psi 2024-11-06T04:58:44Z- handleDepth()- Depth_Keller msg: 1730869124.60 epoch sec; seqNo:3483; depth 50.02 2024-11-06T04:58:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869124.61 epoch sec; seqNo:3483; 4061554.35 north; 594851.92 east; 36.69 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:58:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869121.79 2024-11-06T04:58:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:44Z- waiting for 1730869125.79, time is 1730869124.61 2024-11-06T04:58:45Z- AHRS_M2 msg: 1730869125.00 epoch sec; seqNo:3270 2024-11-06T04:58:45Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.37 psi 2024-11-06T04:58:45Z- handleDepth()- Depth_Keller msg: 1730869125.02 epoch sec; seqNo:3484; depth 50.01 2024-11-06T04:58:45Z- handleDVL() - DVL msg epochMillisec: 1730869125.04 2024-11-06T04:58:45Z- handleDvl() - RDI_Pathfinder msg: 1730869125.04 sec; seqNo:1238; #beams:4 2024-11-06T04:58:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.97 , 31.96 , 41.12 , 38.31 2024-11-06T04:58:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.12 , 0.00 2024-11-06T04:58:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869125.07 epoch sec; seqNo:3484; 4061553.98 north; 594851.92 east; 36.69 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:58:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869121.79 2024-11-06T04:58:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:45Z- waiting for 1730869125.79, time is 1730869125.07 2024-11-06T04:58:45Z- AHRS_M2 msg: 1730869125.41 epoch sec; seqNo:3271 2024-11-06T04:58:45Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.37 psi 2024-11-06T04:58:45Z- handleDepth()- Depth_Keller msg: 1730869125.41 epoch sec; seqNo:3485; depth 49.99 2024-11-06T04:58:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869125.43 epoch sec; seqNo:3485; 4061553.60 north; 594851.93 east; 36.69 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T04:58:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869121.79 2024-11-06T04:58:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:45Z- waiting for 1730869125.79, time is 1730869125.43 2024-11-06T04:58:45Z- handleDepth()- Depth_Keller msg: 1730869125.84 epoch sec; seqNo:3486; depth 49.98 2024-11-06T04:58:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869125.85 epoch sec; seqNo:3486; 4061553.22 north; 594851.93 east; 36.69 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T04:58:45Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:58:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869121.79 2024-11-06T04:58:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:45Z- waiting for 1730869125.79, time is 1730869125.85 2024-11-06T04:58:45Z- LcmTrn::updateTrn() >>>> pose time 1730869125.85, meas time 1730869125.04 <<<< 2024-11-06T04:58:45Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869125.85 2024-11-06T04:58:45Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869125.04 sec, ping # 0; waiting for INS data more recent than 1730869121.79... 2024-11-06T04:58:45Z- TerrainNav::motionUpdate() - currEstimate = (4061553, 594851.93, 49.98) 2024-11-06T04:58:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:58:45Z- TerrainNav:: SubcloudNIS is 0.92, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:58:46Z- TNavPF::Projecting Measurements 2024-11-06T04:58:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:58:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:58:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:58:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:58:46Z- TNavFilter::4 good beams. 2024-11-06T04:58:46Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:58:46Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:58:46Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:58:46Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:58:46Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:58:46Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:58:46Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:58:46Z- TerrainMapOctree: (vehN, vehE ) = (4061553.91, 594852.17). 2024-11-06T04:58:46Z- TerrainMapOctree: Min Distance = 221.11. 2024-11-06T04:58:46Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:58:46Z- Forcing Subcloud Comparison 2024-11-06T04:58:46Z- Weighting particles with subcloud comparison 2024-11-06T04:58:46Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:46Z- meanExpectedMeasDiff: 4.742583 delta_rms_squared: 1.492447 alpha: 0.986776 2024-11-06T04:58:46Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:46Z- meanExpectedMeasDiff: -3.118909 delta_rms_squared: 1.054678 alpha: 0.981390 2024-11-06T04:58:46Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:46Z- meanExpectedMeasDiff: 1.632241 delta_rms_squared: 1.268304 alpha: 0.984476 2024-11-06T04:58:46Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:46Z- meanExpectedMeasDiff: -1.872433 delta_rms_squared: 1.230606 alpha: 0.984008 2024-11-06T04:58:46Z- Subcloud NIS: 0.954045 2024-11-06T04:58:46Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:58:46Z- TNavPF:: sumWeights = 0.010962 2024-11-06T04:58:46Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:58:46Z- TNavPF::First Term of Map Covariance Matrix is 0.25 2024-11-06T04:58:46Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:58:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869125.04 sec. 2024-11-06T04:58:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:58:46Z- variances([0], [2], and psi_berg) = 0.767200 0.308277 0.000000 2024-11-06T04:58:46Z- publishEstimatesDecoder() - at 1730869125.85 sec 2024-11-06T04:58:46Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:58:46Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061494.57 594857.09 62.08 2024-11-06T04:58:46Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061496.52 594856.17 62.03 lat/lon: 36.69 -121.94 2024-11-06T04:58:46Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.77 0.31 0.67 0.00 2024-11-06T04:58:46Z- AHRS_M2 msg: 1730869126.26 epoch sec; seqNo:3272 2024-11-06T04:58:46Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.36 psi 2024-11-06T04:58:46Z- handleDepth()- Depth_Keller msg: 1730869126.27 epoch sec; seqNo:3487; depth 49.97 2024-11-06T04:58:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869126.29 epoch sec; seqNo:3487; 4061552.84 north; 594851.94 east; 36.69 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T04:58:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869125.85 2024-11-06T04:58:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:46Z- waiting for 1730869129.85, time is 1730869126.29 2024-11-06T04:58:46Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:58:46Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:58:46Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:58:46Z- AHRS_M2 msg: 1730869126.61 epoch sec; seqNo:3273 2024-11-06T04:58:46Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.36 psi 2024-11-06T04:58:46Z- handleDepth()- Depth_Keller msg: 1730869126.64 epoch sec; seqNo:3488; depth 49.97 2024-11-06T04:58:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869126.65 epoch sec; seqNo:3488; 4061552.84 north; 594851.94 east; 36.69 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T04:58:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869125.85 2024-11-06T04:58:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:46Z- waiting for 1730869129.85, time is 1730869126.65 2024-11-06T04:58:47Z- AHRS_M2 msg: 1730869127.03 epoch sec; seqNo:3274 2024-11-06T04:58:47Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 3.36 psi 2024-11-06T04:58:47Z- handleDepth()- Depth_Keller msg: 1730869127.04 epoch sec; seqNo:3489; depth 49.96 2024-11-06T04:58:47Z- handleDVL() - DVL msg epochMillisec: 1730869127.06 2024-11-06T04:58:47Z- handleDvl() - RDI_Pathfinder msg: 1730869127.06 sec; seqNo:1239; #beams:4 2024-11-06T04:58:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.97 , 32.15 , 40.93 , 38.69 2024-11-06T04:58:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.13 , 0.03 2024-11-06T04:58:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869127.09 epoch sec; seqNo:3489; 4061552.46 north; 594851.94 east; 36.69 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:58:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869125.85 2024-11-06T04:58:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:47Z- waiting for 1730869129.85, time is 1730869127.09 2024-11-06T04:58:47Z- AHRS_M2 msg: 1730869127.43 epoch sec; seqNo:3275 2024-11-06T04:58:47Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 3.36 psi 2024-11-06T04:58:47Z- handleDepth()- Depth_Keller msg: 1730869127.44 epoch sec; seqNo:3490; depth 49.96 2024-11-06T04:58:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869127.46 epoch sec; seqNo:3490; 4061552.07 north; 594850.72 east; 36.69 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:58:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869125.85 2024-11-06T04:58:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:47Z- waiting for 1730869129.85, time is 1730869127.46 2024-11-06T04:58:47Z- AHRS_M2 msg: 1730869127.84 epoch sec; seqNo:3276 2024-11-06T04:58:47Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.36 psi 2024-11-06T04:58:47Z- handleDepth()- Depth_Keller msg: 1730869127.85 epoch sec; seqNo:3491; depth 49.95 2024-11-06T04:58:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869127.87 epoch sec; seqNo:3491; 4061551.69 north; 594850.73 east; 36.69 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T04:58:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869125.85 2024-11-06T04:58:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:48Z- waiting for 1730869129.85, time is 1730869127.87 2024-11-06T04:58:48Z- AHRS_M2 msg: 1730869128.23 epoch sec; seqNo:3277 2024-11-06T04:58:48Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 3.36 psi 2024-11-06T04:58:48Z- handleDepth()- Depth_Keller msg: 1730869128.25 epoch sec; seqNo:3492; depth 49.94 2024-11-06T04:58:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869128.26 epoch sec; seqNo:3492; 4061551.31 north; 594850.73 east; 36.69 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T04:58:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869125.85 2024-11-06T04:58:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:48Z- waiting for 1730869129.85, time is 1730869128.26 2024-11-06T04:58:48Z- AHRS_M2 msg: 1730869128.64 epoch sec; seqNo:3278 2024-11-06T04:58:48Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 3.36 psi 2024-11-06T04:58:48Z- handleDepth()- Depth_Keller msg: 1730869128.64 epoch sec; seqNo:3493; depth 49.94 2024-11-06T04:58:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869128.65 epoch sec; seqNo:3493; 4061550.93 north; 594850.74 east; 36.69 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T04:58:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869125.85 2024-11-06T04:58:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:48Z- waiting for 1730869129.85, time is 1730869128.65 2024-11-06T04:58:49Z- AHRS_M2 msg: 1730869129.05 epoch sec; seqNo:3279 2024-11-06T04:58:49Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 3.36 psi 2024-11-06T04:58:49Z- handleDepth()- Depth_Keller msg: 1730869129.06 epoch sec; seqNo:3494; depth 49.92 2024-11-06T04:58:49Z- handleDVL() - DVL msg epochMillisec: 1730869129.08 2024-11-06T04:58:49Z- handleDvl() - RDI_Pathfinder msg: 1730869129.08 sec; seqNo:1240; #beams:4 2024-11-06T04:58:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.78 , 32.33 , 40.93 , 37.75 2024-11-06T04:58:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , 0.04 2024-11-06T04:58:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869129.10 epoch sec; seqNo:3494; 4061550.55 north; 594850.74 east; 36.69 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:58:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869125.85 2024-11-06T04:58:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:49Z- waiting for 1730869129.85, time is 1730869129.10 2024-11-06T04:58:49Z- AHRS_M2 msg: 1730869129.45 epoch sec; seqNo:3280 2024-11-06T04:58:49Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 3.35 psi 2024-11-06T04:58:49Z- handleDepth()- Depth_Keller msg: 1730869129.46 epoch sec; seqNo:3495; depth 49.94 2024-11-06T04:58:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869129.48 epoch sec; seqNo:3495; 4061550.17 north; 594850.74 east; 36.69 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T04:58:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869125.85 2024-11-06T04:58:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:49Z- waiting for 1730869129.85, time is 1730869129.48 2024-11-06T04:58:49Z- AHRS_M2 msg: 1730869129.85 epoch sec; seqNo:3281 2024-11-06T04:58:49Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 3.35 psi 2024-11-06T04:58:49Z- handleDepth()- Depth_Keller msg: 1730869129.86 epoch sec; seqNo:3496; depth 49.92 2024-11-06T04:58:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869129.87 epoch sec; seqNo:3496; 4061549.79 north; 594850.75 east; 36.69 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:58:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869125.85 2024-11-06T04:58:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:50Z- waiting for 1730869129.85, time is 1730869129.87 2024-11-06T04:58:50Z- LcmTrn::updateTrn() >>>> pose time 1730869129.87, meas time 1730869129.08 <<<< 2024-11-06T04:58:50Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869129.87 2024-11-06T04:58:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869129.08 sec, ping # 0; waiting for INS data more recent than 1730869125.85... 2024-11-06T04:58:50Z- TerrainNav::motionUpdate() - currEstimate = (4061550, 594850.75, 49.92) 2024-11-06T04:58:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:58:50Z- TerrainNav:: SubcloudNIS is 0.95, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:58:50Z- TNavPF::Projecting Measurements 2024-11-06T04:58:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:58:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:58:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:58:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:58:50Z- TNavFilter::4 good beams. 2024-11-06T04:58:50Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:58:50Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:58:50Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:58:50Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:58:50Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:58:50Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:58:50Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:58:50Z- TerrainMapOctree: (vehN, vehE ) = (4061550.46, 594850.98). 2024-11-06T04:58:50Z- TerrainMapOctree: Min Distance = 223.63. 2024-11-06T04:58:50Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:58:50Z- Forcing Subcloud Comparison 2024-11-06T04:58:50Z- Weighting particles with subcloud comparison 2024-11-06T04:58:50Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:50Z- meanExpectedMeasDiff: 4.226715 delta_rms_squared: 1.512374 alpha: 0.986948 2024-11-06T04:58:50Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:50Z- meanExpectedMeasDiff: -2.739491 delta_rms_squared: 1.052309 alpha: 0.981349 2024-11-06T04:58:50Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:50Z- meanExpectedMeasDiff: 1.715894 delta_rms_squared: 1.438744 alpha: 0.986290 2024-11-06T04:58:50Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:50Z- meanExpectedMeasDiff: -2.634145 delta_rms_squared: 1.240974 alpha: 0.984139 2024-11-06T04:58:50Z- Subcloud NIS: 0.984556 2024-11-06T04:58:50Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:58:50Z- TNavPF:: sumWeights = 0.014497 2024-11-06T04:58:50Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:58:50Z- TNavPF::First Term of Map Covariance Matrix is 0.28 2024-11-06T04:58:50Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:58:50Z- TerrainNav::Resampling particle filter... 2024-11-06T04:58:50Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869129.08 sec. 2024-11-06T04:58:50Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:58:50Z- variances([0], [2], and psi_berg) = 0.783962 0.283550 0.000000 2024-11-06T04:58:50Z- publishEstimatesDecoder() - at 1730869129.87 sec 2024-11-06T04:58:50Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:58:50Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061495.32 594855.16 60.62 2024-11-06T04:58:50Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061493.12 594855.06 62.06 lat/lon: 36.69 -121.94 2024-11-06T04:58:50Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.78 0.28 0.68 0.00 2024-11-06T04:58:50Z- AHRS_M2 msg: 1730869130.26 epoch sec; seqNo:3282 2024-11-06T04:58:50Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.35 psi 2024-11-06T04:58:50Z- handleDepth()- Depth_Keller msg: 1730869130.27 epoch sec; seqNo:3497; depth 49.93 2024-11-06T04:58:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869130.29 epoch sec; seqNo:3497; 4061549.42 north; 594850.75 east; 36.69 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T04:58:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869129.87 2024-11-06T04:58:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:50Z- waiting for 1730869133.87, time is 1730869130.29 2024-11-06T04:58:50Z- AHRS_M2 msg: 1730869130.66 epoch sec; seqNo:3283 2024-11-06T04:58:50Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.35 psi 2024-11-06T04:58:50Z- handleDepth()- Depth_Keller msg: 1730869130.67 epoch sec; seqNo:3498; depth 49.93 2024-11-06T04:58:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869130.68 epoch sec; seqNo:3498; 4061549.42 north; 594850.75 east; 36.69 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T04:58:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869129.87 2024-11-06T04:58:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:50Z- waiting for 1730869133.87, time is 1730869130.68 2024-11-06T04:58:51Z- AHRS_M2 msg: 1730869131.07 epoch sec; seqNo:3284 2024-11-06T04:58:51Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.35 psi 2024-11-06T04:58:51Z- handleDepth()- Depth_Keller msg: 1730869131.08 epoch sec; seqNo:3499; depth 49.92 2024-11-06T04:58:51Z- handleDVL() - DVL msg epochMillisec: 1730869131.10 2024-11-06T04:58:51Z- handleDvl() - RDI_Pathfinder msg: 1730869131.10 sec; seqNo:1241; #beams:4 2024-11-06T04:58:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.59 , 32.71 , 40.93 , 37.75 2024-11-06T04:58:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.08 , 0.04 2024-11-06T04:58:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869131.13 epoch sec; seqNo:3499; 4061549.04 north; 594850.76 east; 36.69 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:58:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869129.87 2024-11-06T04:58:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:51Z- waiting for 1730869133.87, time is 1730869131.13 2024-11-06T04:58:51Z- AHRS_M2 msg: 1730869131.47 epoch sec; seqNo:3285 2024-11-06T04:58:51Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.35 psi 2024-11-06T04:58:51Z- handleDepth()- Depth_Keller msg: 1730869131.48 epoch sec; seqNo:3500; depth 49.93 2024-11-06T04:58:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869131.50 epoch sec; seqNo:3500; 4061548.66 north; 594850.76 east; 36.69 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T04:58:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869129.87 2024-11-06T04:58:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:51Z- waiting for 1730869133.87, time is 1730869131.50 2024-11-06T04:58:51Z- AHRS_M2 msg: 1730869131.87 epoch sec; seqNo:3286 2024-11-06T04:58:51Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.35 psi 2024-11-06T04:58:51Z- handleDepth()- Depth_Keller msg: 1730869131.88 epoch sec; seqNo:3501; depth 49.92 2024-11-06T04:58:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869131.89 epoch sec; seqNo:3501; 4061548.27 north; 594849.55 east; 36.69 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:58:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869129.87 2024-11-06T04:58:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:52Z- waiting for 1730869133.87, time is 1730869131.89 2024-11-06T04:58:52Z- AHRS_M2 msg: 1730869132.28 epoch sec; seqNo:3287 2024-11-06T04:58:52Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.35 psi 2024-11-06T04:58:52Z- handleDepth()- Depth_Keller msg: 1730869132.28 epoch sec; seqNo:3502; depth 49.92 2024-11-06T04:58:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869132.29 epoch sec; seqNo:3502; 4061547.89 north; 594849.55 east; 36.69 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:58:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869129.87 2024-11-06T04:58:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:52Z- waiting for 1730869133.87, time is 1730869132.29 2024-11-06T04:58:52Z- AHRS_M2 msg: 1730869132.68 epoch sec; seqNo:3288 2024-11-06T04:58:52Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.35 psi 2024-11-06T04:58:52Z- handleDepth()- Depth_Keller msg: 1730869132.72 epoch sec; seqNo:3503; depth 49.92 2024-11-06T04:58:52Z- handleDVL() - DVL msg epochMillisec: 1730869132.76 2024-11-06T04:58:52Z- handleDvl() - RDI_Pathfinder msg: 1730869132.76 sec; seqNo:1242; #beams:4 2024-11-06T04:58:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.78 , 32.89 , 41.12 , 37.19 2024-11-06T04:58:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , 0.03 2024-11-06T04:58:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869132.78 epoch sec; seqNo:3503; 4061547.51 north; 594849.55 east; 36.69 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:58:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869129.87 2024-11-06T04:58:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:52Z- waiting for 1730869133.87, time is 1730869132.78 2024-11-06T04:58:53Z- AHRS_M2 msg: 1730869133.08 epoch sec; seqNo:3289 2024-11-06T04:58:53Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 3.35 psi 2024-11-06T04:58:53Z- handleDepth()- Depth_Keller msg: 1730869133.10 epoch sec; seqNo:3504; depth 49.93 2024-11-06T04:58:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869133.12 epoch sec; seqNo:3504; 4061547.13 north; 594849.56 east; 36.69 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T04:58:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869129.87 2024-11-06T04:58:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:53Z- waiting for 1730869133.87, time is 1730869133.12 2024-11-06T04:58:53Z- AHRS_M2 msg: 1730869133.50 epoch sec; seqNo:3290 2024-11-06T04:58:53Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.35 psi 2024-11-06T04:58:53Z- handleDepth()- Depth_Keller msg: 1730869133.50 epoch sec; seqNo:3505; depth 49.91 2024-11-06T04:58:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869133.52 epoch sec; seqNo:3505; 4061546.75 north; 594849.56 east; 36.69 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:58:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869129.87 2024-11-06T04:58:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:53Z- waiting for 1730869133.87, time is 1730869133.52 2024-11-06T04:58:53Z- AHRS_M2 msg: 1730869133.89 epoch sec; seqNo:3291 2024-11-06T04:58:53Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.35 psi 2024-11-06T04:58:53Z- handleDepth()- Depth_Keller msg: 1730869133.90 epoch sec; seqNo:3506; depth 49.91 2024-11-06T04:58:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869133.91 epoch sec; seqNo:3506; 4061546.37 north; 594849.57 east; 36.69 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:58:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869129.87 2024-11-06T04:58:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:54Z- waiting for 1730869133.87, time is 1730869133.91 2024-11-06T04:58:54Z- LcmTrn::updateTrn() >>>> pose time 1730869133.91, meas time 1730869132.76 <<<< 2024-11-06T04:58:54Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869133.91 2024-11-06T04:58:54Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869132.76 sec, ping # 0; waiting for INS data more recent than 1730869129.87... 2024-11-06T04:58:54Z- TerrainNav::motionUpdate() - currEstimate = (4061546, 594849.57, 49.91) 2024-11-06T04:58:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:58:54Z- TerrainNav:: SubcloudNIS is 0.98, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:58:54Z- TNavPF::Projecting Measurements 2024-11-06T04:58:54Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:58:54Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:58:54Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:58:54Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:58:54Z- TNavFilter::4 good beams. 2024-11-06T04:58:54Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:58:54Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:58:54Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:58:54Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:58:54Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:58:54Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:58:54Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:58:54Z- TerrainMapOctree: (vehN, vehE ) = (4061547.34, 594849.90). 2024-11-06T04:58:54Z- TerrainMapOctree: Min Distance = 225.95. 2024-11-06T04:58:54Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:58:54Z- Forcing Subcloud Comparison 2024-11-06T04:58:54Z- Weighting particles with subcloud comparison 2024-11-06T04:58:54Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:54Z- meanExpectedMeasDiff: 4.302159 delta_rms_squared: 1.526821 alpha: 0.987070 2024-11-06T04:58:54Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:54Z- meanExpectedMeasDiff: -2.077079 delta_rms_squared: 1.060130 alpha: 0.981484 2024-11-06T04:58:54Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:54Z- meanExpectedMeasDiff: 2.552963 delta_rms_squared: 1.946666 alpha: 0.989831 2024-11-06T04:58:54Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:54Z- meanExpectedMeasDiff: -3.201604 delta_rms_squared: 1.258770 alpha: 0.984360 2024-11-06T04:58:54Z- Subcloud NIS: 1.014953 2024-11-06T04:58:54Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:58:54Z- TNavPF:: sumWeights = 0.009427 2024-11-06T04:58:54Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:58:54Z- TNavPF::First Term of Map Covariance Matrix is 0.17 2024-11-06T04:58:54Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:58:54Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869132.76 sec. 2024-11-06T04:58:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:58:54Z- variances([0], [2], and psi_berg) = 0.815175 0.294993 0.000000 2024-11-06T04:58:54Z- publishEstimatesDecoder() - at 1730869133.91 sec 2024-11-06T04:58:54Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:58:54Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061488.50 594853.84 61.54 2024-11-06T04:58:54Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061489.56 594853.96 61.99 lat/lon: 36.69 -121.94 2024-11-06T04:58:54Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.82 0.29 0.66 0.00 2024-11-06T04:58:54Z- handleDepth()- Depth_Keller msg: 1730869134.31 epoch sec; seqNo:3507; depth 49.90 2024-11-06T04:58:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869134.32 epoch sec; seqNo:3507; 4061545.99 north; 594849.57 east; 36.69 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:58:54Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:58:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869133.91 2024-11-06T04:58:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:54Z- waiting for 1730869137.91, time is 1730869134.32 2024-11-06T04:58:54Z- AHRS_M2 msg: 1730869134.74 epoch sec; seqNo:3292 2024-11-06T04:58:54Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.35 psi 2024-11-06T04:58:54Z- handleDepth()- Depth_Keller msg: 1730869134.75 epoch sec; seqNo:3508; depth 49.91 2024-11-06T04:58:54Z- handleDVL() - DVL msg epochMillisec: 1730869134.78 2024-11-06T04:58:54Z- handleDvl() - RDI_Pathfinder msg: 1730869134.78 sec; seqNo:1243; #beams:4 2024-11-06T04:58:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.78 , 33.08 , 40.93 , 36.82 2024-11-06T04:58:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , -0.00 2024-11-06T04:58:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869134.79 epoch sec; seqNo:3508; 4061545.61 north; 594849.57 east; 36.69 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T04:58:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869133.91 2024-11-06T04:58:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:54Z- waiting for 1730869137.91, time is 1730869134.79 2024-11-06T04:58:55Z- AHRS_M2 msg: 1730869135.11 epoch sec; seqNo:3293 2024-11-06T04:58:55Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.35 psi 2024-11-06T04:58:55Z- handleDepth()- Depth_Keller msg: 1730869135.11 epoch sec; seqNo:3509; depth 49.90 2024-11-06T04:58:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869135.12 epoch sec; seqNo:3509; 4061545.24 north; 594849.58 east; 36.69 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T04:58:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869133.91 2024-11-06T04:58:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:55Z- waiting for 1730869137.91, time is 1730869135.12 2024-11-06T04:58:55Z- AHRS_M2 msg: 1730869135.51 epoch sec; seqNo:3294 2024-11-06T04:58:55Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.35 psi 2024-11-06T04:58:55Z- handleDepth()- Depth_Keller msg: 1730869135.52 epoch sec; seqNo:3510; depth 49.89 2024-11-06T04:58:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869135.54 epoch sec; seqNo:3510; 4061544.86 north; 594849.58 east; 36.69 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:58:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869133.91 2024-11-06T04:58:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:55Z- waiting for 1730869137.91, time is 1730869135.54 2024-11-06T04:58:55Z- AHRS_M2 msg: 1730869135.91 epoch sec; seqNo:3295 2024-11-06T04:58:55Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.35 psi 2024-11-06T04:58:55Z- handleDepth()- Depth_Keller msg: 1730869135.92 epoch sec; seqNo:3511; depth 49.89 2024-11-06T04:58:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869135.93 epoch sec; seqNo:3511; 4061544.86 north; 594849.58 east; 36.69 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:58:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869133.91 2024-11-06T04:58:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:56Z- waiting for 1730869137.91, time is 1730869135.93 2024-11-06T04:58:56Z- AHRS_M2 msg: 1730869136.32 epoch sec; seqNo:3296 2024-11-06T04:58:56Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.35 psi 2024-11-06T04:58:56Z- handleDepth()- Depth_Keller msg: 1730869136.33 epoch sec; seqNo:3512; depth 49.88 2024-11-06T04:58:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869136.34 epoch sec; seqNo:3512; 4061544.48 north; 594849.59 east; 36.69 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T04:58:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869133.91 2024-11-06T04:58:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:56Z- waiting for 1730869137.91, time is 1730869136.34 2024-11-06T04:58:56Z- AHRS_M2 msg: 1730869136.75 epoch sec; seqNo:3297 2024-11-06T04:58:56Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.35 psi 2024-11-06T04:58:56Z- handleDepth()- Depth_Keller msg: 1730869136.76 epoch sec; seqNo:3513; depth 49.87 2024-11-06T04:58:56Z- handleDVL() - DVL msg epochMillisec: 1730869136.78 2024-11-06T04:58:56Z- handleDvl() - RDI_Pathfinder msg: 1730869136.78 sec; seqNo:1244; #beams:4 2024-11-06T04:58:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.97 , 33.27 , 41.12 , 37.38 2024-11-06T04:58:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.08 , -0.02 2024-11-06T04:58:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869136.81 epoch sec; seqNo:3513; 4061544.10 north; 594849.59 east; 36.69 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:58:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869133.91 2024-11-06T04:58:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:56Z- waiting for 1730869137.91, time is 1730869136.81 2024-11-06T04:58:57Z- AHRS_M2 msg: 1730869137.12 epoch sec; seqNo:3298 2024-11-06T04:58:57Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.35 psi 2024-11-06T04:58:57Z- handleDepth()- Depth_Keller msg: 1730869137.13 epoch sec; seqNo:3514; depth 49.87 2024-11-06T04:58:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869137.14 epoch sec; seqNo:3514; 4061543.71 north; 594848.38 east; 36.69 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:58:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869133.91 2024-11-06T04:58:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:57Z- waiting for 1730869137.91, time is 1730869137.14 2024-11-06T04:58:57Z- AHRS_M2 msg: 1730869137.53 epoch sec; seqNo:3299 2024-11-06T04:58:57Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 3.35 psi 2024-11-06T04:58:57Z- handleDepth()- Depth_Keller msg: 1730869137.53 epoch sec; seqNo:3515; depth 49.87 2024-11-06T04:58:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869137.54 epoch sec; seqNo:3515; 4061543.33 north; 594848.38 east; 36.69 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:58:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869133.91 2024-11-06T04:58:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:57Z- waiting for 1730869137.91, time is 1730869137.54 2024-11-06T04:58:57Z- AHRS_M2 msg: 1730869137.94 epoch sec; seqNo:3300 2024-11-06T04:58:57Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 3.35 psi 2024-11-06T04:58:57Z- handleDepth()- Depth_Keller msg: 1730869137.94 epoch sec; seqNo:3516; depth 49.86 2024-11-06T04:58:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869137.95 epoch sec; seqNo:3516; 4061542.95 north; 594848.38 east; 36.69 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:58:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869133.91 2024-11-06T04:58:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:58Z- waiting for 1730869137.91, time is 1730869137.95 2024-11-06T04:58:58Z- LcmTrn::updateTrn() >>>> pose time 1730869137.95, meas time 1730869136.78 <<<< 2024-11-06T04:58:58Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869137.95 2024-11-06T04:58:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869136.78 sec, ping # 0; waiting for INS data more recent than 1730869133.91... 2024-11-06T04:58:58Z- TerrainNav::motionUpdate() - currEstimate = (4061543, 594848.38, 49.86) 2024-11-06T04:58:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:58:58Z- TerrainNav:: SubcloudNIS is 1.01, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:58:58Z- TNavPF::Projecting Measurements 2024-11-06T04:58:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:58:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:58:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:58:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:58:58Z- TNavFilter::4 good beams. 2024-11-06T04:58:58Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:58:58Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:58:58Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:58:58Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:58:58Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:58:58Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:58:58Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:58:58Z- TerrainMapOctree: (vehN, vehE ) = (4061543.94, 594848.73). 2024-11-06T04:58:58Z- TerrainMapOctree: Min Distance = 228.49. 2024-11-06T04:58:58Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:58:58Z- Forcing Subcloud Comparison 2024-11-06T04:58:58Z- Weighting particles with subcloud comparison 2024-11-06T04:58:58Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:58Z- meanExpectedMeasDiff: 4.342980 delta_rms_squared: 1.477255 alpha: 0.986642 2024-11-06T04:58:58Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:58Z- meanExpectedMeasDiff: -1.805548 delta_rms_squared: 1.022814 alpha: 0.980821 2024-11-06T04:58:58Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:58Z- meanExpectedMeasDiff: 2.711960 delta_rms_squared: 1.335243 alpha: 0.985242 2024-11-06T04:58:58Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:58:58Z- meanExpectedMeasDiff: -3.134681 delta_rms_squared: 1.328957 alpha: 0.985174 2024-11-06T04:58:58Z- Subcloud NIS: 1.016477 2024-11-06T04:58:58Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:58:58Z- TNavPF:: sumWeights = 0.010812 2024-11-06T04:58:58Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:58:58Z- TNavPF::First Term of Map Covariance Matrix is 0.20 2024-11-06T04:58:58Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T04:58:58Z- TerrainNav::Resampling particle filter... 2024-11-06T04:58:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869136.78 sec. 2024-11-06T04:58:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:58:58Z- variances([0], [2], and psi_berg) = 0.839504 0.368091 0.000000 2024-11-06T04:58:58Z- publishEstimatesDecoder() - at 1730869137.95 sec 2024-11-06T04:58:58Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:58:58Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061488.61 594852.90 61.84 2024-11-06T04:58:58Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061486.14 594852.83 61.85 lat/lon: 36.69 -121.94 2024-11-06T04:58:58Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.84 0.37 0.66 0.00 2024-11-06T04:58:58Z- AHRS_M2 msg: 1730869138.34 epoch sec; seqNo:3301 2024-11-06T04:58:58Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.35 psi 2024-11-06T04:58:58Z- handleDepth()- Depth_Keller msg: 1730869138.34 epoch sec; seqNo:3517; depth 49.86 2024-11-06T04:58:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869138.37 epoch sec; seqNo:3517; 4061542.57 north; 594848.39 east; 36.69 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T04:58:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869137.95 2024-11-06T04:58:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:58Z- waiting for 1730869141.95, time is 1730869138.37 2024-11-06T04:58:58Z- AHRS_M2 msg: 1730869138.76 epoch sec; seqNo:3302 2024-11-06T04:58:58Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.34 psi 2024-11-06T04:58:58Z- handleDepth()- Depth_Keller msg: 1730869138.76 epoch sec; seqNo:3518; depth 49.85 2024-11-06T04:58:58Z- handleDVL() - DVL msg epochMillisec: 1730869138.79 2024-11-06T04:58:58Z- handleDvl() - RDI_Pathfinder msg: 1730869138.79 sec; seqNo:1245; #beams:4 2024-11-06T04:58:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.15 , 33.45 , 41.30 , 37.94 2024-11-06T04:58:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.12 , -0.01 2024-11-06T04:58:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869138.80 epoch sec; seqNo:3518; 4061542.19 north; 594848.39 east; 36.69 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:58:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:58:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869137.95 2024-11-06T04:58:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:58Z- waiting for 1730869141.95, time is 1730869138.80 2024-11-06T04:58:59Z- AHRS_M2 msg: 1730869139.15 epoch sec; seqNo:3303 2024-11-06T04:58:59Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.34 psi 2024-11-06T04:58:59Z- handleDepth()- Depth_Keller msg: 1730869139.15 epoch sec; seqNo:3519; depth 49.83 2024-11-06T04:58:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869139.17 epoch sec; seqNo:3519; 4061541.81 north; 594848.40 east; 36.69 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:58:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869137.95 2024-11-06T04:58:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:59Z- waiting for 1730869141.95, time is 1730869139.17 2024-11-06T04:58:59Z- AHRS_M2 msg: 1730869139.55 epoch sec; seqNo:3304 2024-11-06T04:58:59Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.34 psi 2024-11-06T04:58:59Z- handleDepth()- Depth_Keller msg: 1730869139.56 epoch sec; seqNo:3520; depth 49.84 2024-11-06T04:58:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869139.57 epoch sec; seqNo:3520; 4061541.43 north; 594848.40 east; 36.69 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T04:58:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:58:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869137.95 2024-11-06T04:58:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:58:59Z- waiting for 1730869141.95, time is 1730869139.57 2024-11-06T04:58:59Z- AHRS_M2 msg: 1730869139.95 epoch sec; seqNo:3305 2024-11-06T04:58:59Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.34 psi 2024-11-06T04:58:59Z- handleDepth()- Depth_Keller msg: 1730869139.95 epoch sec; seqNo:3521; depth 49.82 2024-11-06T04:58:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869139.96 epoch sec; seqNo:3521; 4061541.05 north; 594848.40 east; 36.69 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:59:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869137.95 2024-11-06T04:59:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:00Z- waiting for 1730869141.95, time is 1730869139.96 2024-11-06T04:59:00Z- handleDepth()- Depth_Keller msg: 1730869140.37 epoch sec; seqNo:3522; depth 49.82 2024-11-06T04:59:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869140.39 epoch sec; seqNo:3522; 4061540.68 north; 594848.41 east; 36.69 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:59:00Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:59:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869137.95 2024-11-06T04:59:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:00Z- waiting for 1730869141.95, time is 1730869140.39 2024-11-06T04:59:00Z- AHRS_M2 msg: 1730869140.76 epoch sec; seqNo:3306 2024-11-06T04:59:00Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.34 psi 2024-11-06T04:59:00Z- handleDepth()- Depth_Keller msg: 1730869140.77 epoch sec; seqNo:3523; depth 49.83 2024-11-06T04:59:00Z- handleDVL() - DVL msg epochMillisec: 1730869140.79 2024-11-06T04:59:00Z- handleDvl() - RDI_Pathfinder msg: 1730869140.79 sec; seqNo:1246; #beams:4 2024-11-06T04:59:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.22 , 32.89 , 41.30 , 38.31 2024-11-06T04:59:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.11 , 0.02 2024-11-06T04:59:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869140.80 epoch sec; seqNo:3523; 4061540.68 north; 594848.41 east; 36.69 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:59:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869137.95 2024-11-06T04:59:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:00Z- waiting for 1730869141.95, time is 1730869140.80 2024-11-06T04:59:01Z- AHRS_M2 msg: 1730869141.16 epoch sec; seqNo:3307 2024-11-06T04:59:01Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.34 psi 2024-11-06T04:59:01Z- handleDepth()- Depth_Keller msg: 1730869141.17 epoch sec; seqNo:3524; depth 49.84 2024-11-06T04:59:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869141.20 epoch sec; seqNo:3524; 4061540.30 north; 594848.41 east; 36.69 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T04:59:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869137.95 2024-11-06T04:59:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:01Z- waiting for 1730869141.95, time is 1730869141.20 2024-11-06T04:59:01Z- handleDepth()- Depth_Keller msg: 1730869141.59 epoch sec; seqNo:3525; depth 49.82 2024-11-06T04:59:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869141.60 epoch sec; seqNo:3525; 4061539.90 north; 594847.20 east; 36.69 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:59:01Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:59:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869137.95 2024-11-06T04:59:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:01Z- waiting for 1730869141.95, time is 1730869141.60 2024-11-06T04:59:01Z- AHRS_M2 msg: 1730869141.97 epoch sec; seqNo:3308 2024-11-06T04:59:01Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.34 psi 2024-11-06T04:59:01Z- handleDepth()- Depth_Keller msg: 1730869141.98 epoch sec; seqNo:3526; depth 49.83 2024-11-06T04:59:01Z- handleDVL() - DVL msg epochMillisec: 1730869141.98 2024-11-06T04:59:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:59:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:59:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:59:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:59:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:59:01Z- handleDvl() - RDI_Pathfinder msg: 1730869141.98 sec; seqNo:1247; #beams:0 2024-11-06T04:59:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:59:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.11 , 0.02 2024-11-06T04:59:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869142.00 epoch sec; seqNo:3526; 4061539.53 north; 594847.20 east; 36.69 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:59:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869137.95 2024-11-06T04:59:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:02Z- waiting for 1730869141.95, time is 1730869142.00 2024-11-06T04:59:02Z- LcmTrn::updateTrn() >>>> pose time 1730869142.00, meas time 1730869141.98 <<<< 2024-11-06T04:59:02Z- LcmTrn::updateTrn() equating pose 1730869141.98 and meas times 1730869141.98 2024-11-06T04:59:02Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869141.98 2024-11-06T04:59:02Z- TerrainNav::motionUpdate() - currEstimate = (4061540, 594847.20, 49.83) 2024-11-06T04:59:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:59:02Z- TerrainNav:: SubcloudNIS is 1.02, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:59:02Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:59:02Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:59:02Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:59:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:59:02Z- variances([0], [2], and psi_berg) = 0.840788 0.369490 0.000000 2024-11-06T04:59:02Z- publishEstimatesDecoder() - at 1730869141.98 sec 2024-11-06T04:59:02Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:59:02Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061485.17 594851.68 61.48 2024-11-06T04:59:02Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061482.71 594851.64 61.82 lat/lon: 36.69 -121.94 2024-11-06T04:59:02Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.84 0.37 0.92 0.00 2024-11-06T04:59:02Z- AHRS_M2 msg: 1730869142.38 epoch sec; seqNo:3309 2024-11-06T04:59:02Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.34 psi 2024-11-06T04:59:02Z- handleDepth()- Depth_Keller msg: 1730869142.40 epoch sec; seqNo:3527; depth 49.81 2024-11-06T04:59:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869142.40 epoch sec; seqNo:3527; 4061539.15 north; 594847.21 east; 36.69 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:59:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869141.98 2024-11-06T04:59:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:02Z- waiting for 1730869145.98, time is 1730869142.40 2024-11-06T04:59:02Z- AHRS_M2 msg: 1730869142.78 epoch sec; seqNo:3310 2024-11-06T04:59:02Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 3.34 psi 2024-11-06T04:59:02Z- handleDepth()- Depth_Keller msg: 1730869142.79 epoch sec; seqNo:3528; depth 49.83 2024-11-06T04:59:02Z- handleDVL() - DVL msg epochMillisec: 1730869142.83 2024-11-06T04:59:02Z- handleDvl() - RDI_Pathfinder msg: 1730869142.83 sec; seqNo:1248; #beams:4 2024-11-06T04:59:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.03 , 32.71 , 41.30 , 38.31 2024-11-06T04:59:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.09 , 0.04 2024-11-06T04:59:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869142.84 epoch sec; seqNo:3528; 4061538.77 north; 594847.21 east; 36.69 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:59:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869141.98 2024-11-06T04:59:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:02Z- waiting for 1730869145.98, time is 1730869142.84 2024-11-06T04:59:03Z- AHRS_M2 msg: 1730869143.18 epoch sec; seqNo:3311 2024-11-06T04:59:03Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 3.34 psi 2024-11-06T04:59:03Z- handleDepth()- Depth_Keller msg: 1730869143.19 epoch sec; seqNo:3529; depth 49.83 2024-11-06T04:59:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869143.22 epoch sec; seqNo:3529; 4061538.39 north; 594847.21 east; 36.69 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:59:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869141.98 2024-11-06T04:59:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:03Z- waiting for 1730869145.98, time is 1730869143.22 2024-11-06T04:59:03Z- AHRS_M2 msg: 1730869143.59 epoch sec; seqNo:3312 2024-11-06T04:59:03Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 3.34 psi 2024-11-06T04:59:03Z- handleDepth()- Depth_Keller msg: 1730869143.60 epoch sec; seqNo:3530; depth 49.81 2024-11-06T04:59:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869143.62 epoch sec; seqNo:3530; 4061538.01 north; 594847.22 east; 36.69 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:59:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869141.98 2024-11-06T04:59:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:03Z- waiting for 1730869145.98, time is 1730869143.62 2024-11-06T04:59:04Z- AHRS_M2 msg: 1730869143.99 epoch sec; seqNo:3313 2024-11-06T04:59:04Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 3.34 psi 2024-11-06T04:59:04Z- handleDepth()- Depth_Keller msg: 1730869144.00 epoch sec; seqNo:3531; depth 49.81 2024-11-06T04:59:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869144.02 epoch sec; seqNo:3531; 4061537.63 north; 594847.22 east; 36.69 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:59:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869141.98 2024-11-06T04:59:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:04Z- waiting for 1730869145.98, time is 1730869144.02 2024-11-06T04:59:04Z- AHRS_M2 msg: 1730869144.40 epoch sec; seqNo:3314 2024-11-06T04:59:04Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 3.34 psi 2024-11-06T04:59:04Z- handleDepth()- Depth_Keller msg: 1730869144.41 epoch sec; seqNo:3532; depth 49.82 2024-11-06T04:59:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869144.42 epoch sec; seqNo:3532; 4061537.25 north; 594847.23 east; 36.69 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:59:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869141.98 2024-11-06T04:59:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:04Z- waiting for 1730869145.98, time is 1730869144.42 2024-11-06T04:59:04Z- AHRS_M2 msg: 1730869144.81 epoch sec; seqNo:3315 2024-11-06T04:59:04Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 3.34 psi 2024-11-06T04:59:04Z- handleDepth()- Depth_Keller msg: 1730869144.81 epoch sec; seqNo:3533; depth 49.81 2024-11-06T04:59:04Z- handleDVL() - DVL msg epochMillisec: 1730869144.84 2024-11-06T04:59:04Z- handleDvl() - RDI_Pathfinder msg: 1730869144.84 sec; seqNo:1249; #beams:4 2024-11-06T04:59:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.85 , 32.71 , 41.68 , 38.69 2024-11-06T04:59:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.05 , 0.03 2024-11-06T04:59:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869144.87 epoch sec; seqNo:3533; 4061536.87 north; 594847.23 east; 36.69 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:59:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869141.98 2024-11-06T04:59:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:04Z- waiting for 1730869145.98, time is 1730869144.87 2024-11-06T04:59:05Z- AHRS_M2 msg: 1730869145.20 epoch sec; seqNo:3316 2024-11-06T04:59:05Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 3.34 psi 2024-11-06T04:59:05Z- handleDepth()- Depth_Keller msg: 1730869145.22 epoch sec; seqNo:3534; depth 49.80 2024-11-06T04:59:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869145.24 epoch sec; seqNo:3534; 4061536.87 north; 594847.23 east; 36.69 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T04:59:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869141.98 2024-11-06T04:59:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:05Z- waiting for 1730869145.98, time is 1730869145.24 2024-11-06T04:59:05Z- AHRS_M2 msg: 1730869145.61 epoch sec; seqNo:3317 2024-11-06T04:59:05Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.34 psi 2024-11-06T04:59:05Z- handleDepth()- Depth_Keller msg: 1730869145.62 epoch sec; seqNo:3535; depth 49.81 2024-11-06T04:59:05Z- handleDVL() - DVL msg epochMillisec: 1730869145.62 2024-11-06T04:59:05Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:05Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:05Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:05Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:59:05Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:59:05Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:59:05Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:59:05Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:59:05Z- handleDvl() - RDI_Pathfinder msg: 1730869145.62 sec; seqNo:1250; #beams:0 2024-11-06T04:59:05Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:59:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.05 , 0.03 2024-11-06T04:59:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869145.63 epoch sec; seqNo:3535; 4061536.49 north; 594847.23 east; 36.69 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:59:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869141.98 2024-11-06T04:59:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:05Z- waiting for 1730869145.98, time is 1730869145.63 2024-11-06T04:59:06Z- AHRS_M2 msg: 1730869146.02 epoch sec; seqNo:3318 2024-11-06T04:59:06Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.34 psi 2024-11-06T04:59:06Z- handleDepth()- Depth_Keller msg: 1730869146.02 epoch sec; seqNo:3536; depth 49.82 2024-11-06T04:59:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869146.03 epoch sec; seqNo:3536; 4061536.12 north; 594847.24 east; 36.69 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:59:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869141.98 2024-11-06T04:59:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:06Z- waiting for 1730869145.98, time is 1730869146.03 2024-11-06T04:59:06Z- LcmTrn::updateTrn() >>>> pose time 1730869146.03, meas time 1730869145.62 <<<< 2024-11-06T04:59:06Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869146.03 2024-11-06T04:59:06Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:59:06Z- TerrainNav::motionUpdate() - currEstimate = (4061536, 594847.24, 49.82) 2024-11-06T04:59:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:59:06Z- TerrainNav:: SubcloudNIS is 1.02, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:59:06Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:59:06Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:59:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:59:06Z- variances([0], [2], and psi_berg) = 0.842305 0.371037 0.000000 2024-11-06T04:59:06Z- publishEstimatesDecoder() - at 1730869146.03 sec 2024-11-06T04:59:06Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:59:06Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061481.67 594851.75 61.50 2024-11-06T04:59:06Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061479.30 594851.68 61.81 lat/lon: 36.69 -121.94 2024-11-06T04:59:06Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.84 0.37 1.17 0.00 2024-11-06T04:59:06Z- AHRS_M2 msg: 1730869146.41 epoch sec; seqNo:3319 2024-11-06T04:59:06Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.34 psi 2024-11-06T04:59:06Z- handleDepth()- Depth_Keller msg: 1730869146.44 epoch sec; seqNo:3537; depth 49.81 2024-11-06T04:59:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869146.45 epoch sec; seqNo:3537; 4061535.72 north; 594846.02 east; 36.69 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:59:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869146.03 2024-11-06T04:59:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:06Z- waiting for 1730869150.03, time is 1730869146.45 2024-11-06T04:59:06Z- AHRS_M2 msg: 1730869146.82 epoch sec; seqNo:3320 2024-11-06T04:59:06Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.34 psi 2024-11-06T04:59:06Z- handleDepth()- Depth_Keller msg: 1730869146.83 epoch sec; seqNo:3538; depth 49.79 2024-11-06T04:59:06Z- handleDVL() - DVL msg epochMillisec: 1730869146.86 2024-11-06T04:59:06Z- handleDvl() - RDI_Pathfinder msg: 1730869146.86 sec; seqNo:1251; #beams:4 2024-11-06T04:59:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.85 , 32.89 , 41.87 , 38.69 2024-11-06T04:59:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , 0.01 2024-11-06T04:59:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869146.88 epoch sec; seqNo:3538; 4061535.34 north; 594846.03 east; 36.69 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:59:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869146.03 2024-11-06T04:59:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:06Z- waiting for 1730869150.03, time is 1730869146.88 2024-11-06T04:59:07Z- AHRS_M2 msg: 1730869147.23 epoch sec; seqNo:3321 2024-11-06T04:59:07Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 3.34 psi 2024-11-06T04:59:07Z- handleDepth()- Depth_Keller msg: 1730869147.23 epoch sec; seqNo:3539; depth 49.80 2024-11-06T04:59:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869147.25 epoch sec; seqNo:3539; 4061534.97 north; 594846.03 east; 36.69 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T04:59:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869146.03 2024-11-06T04:59:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:07Z- waiting for 1730869150.03, time is 1730869147.25 2024-11-06T04:59:07Z- AHRS_M2 msg: 1730869147.63 epoch sec; seqNo:3322 2024-11-06T04:59:07Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 3.34 psi 2024-11-06T04:59:07Z- handleDepth()- Depth_Keller msg: 1730869147.64 epoch sec; seqNo:3540; depth 49.79 2024-11-06T04:59:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869147.65 epoch sec; seqNo:3540; 4061534.59 north; 594846.04 east; 36.69 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:59:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869146.03 2024-11-06T04:59:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:07Z- waiting for 1730869150.03, time is 1730869147.65 2024-11-06T04:59:08Z- AHRS_M2 msg: 1730869148.03 epoch sec; seqNo:3323 2024-11-06T04:59:08Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 3.34 psi 2024-11-06T04:59:08Z- handleDepth()- Depth_Keller msg: 1730869148.04 epoch sec; seqNo:3541; depth 49.78 2024-11-06T04:59:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869148.07 epoch sec; seqNo:3541; 4061534.21 north; 594846.04 east; 36.69 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:59:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869146.03 2024-11-06T04:59:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:08Z- waiting for 1730869150.03, time is 1730869148.07 2024-11-06T04:59:08Z- AHRS_M2 msg: 1730869148.44 epoch sec; seqNo:3324 2024-11-06T04:59:08Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 3.34 psi 2024-11-06T04:59:08Z- handleDepth()- Depth_Keller msg: 1730869148.44 epoch sec; seqNo:3542; depth 49.77 2024-11-06T04:59:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869148.45 epoch sec; seqNo:3542; 4061533.83 north; 594846.04 east; 36.69 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T04:59:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869146.03 2024-11-06T04:59:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:08Z- waiting for 1730869150.03, time is 1730869148.45 2024-11-06T04:59:08Z- AHRS_M2 msg: 1730869148.84 epoch sec; seqNo:3325 2024-11-06T04:59:08Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 3.34 psi 2024-11-06T04:59:08Z- handleDepth()- Depth_Keller msg: 1730869148.85 epoch sec; seqNo:3543; depth 49.76 2024-11-06T04:59:08Z- handleDVL() - DVL msg epochMillisec: 1730869148.87 2024-11-06T04:59:08Z- handleDvl() - RDI_Pathfinder msg: 1730869148.87 sec; seqNo:1252; #beams:4 2024-11-06T04:59:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.66 , 33.45 , 41.68 , 38.87 2024-11-06T04:59:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.02 2024-11-06T04:59:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869148.89 epoch sec; seqNo:3543; 4061533.45 north; 594846.05 east; 36.69 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T04:59:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869146.03 2024-11-06T04:59:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:09Z- waiting for 1730869150.03, time is 1730869148.89 2024-11-06T04:59:09Z- AHRS_M2 msg: 1730869149.25 epoch sec; seqNo:3326 2024-11-06T04:59:09Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 3.34 psi 2024-11-06T04:59:09Z- handleDepth()- Depth_Keller msg: 1730869149.28 epoch sec; seqNo:3544; depth 49.76 2024-11-06T04:59:09Z- handleDVL() - DVL msg epochMillisec: 1730869149.28 2024-11-06T04:59:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:59:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:59:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:59:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:59:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:59:09Z- handleDvl() - RDI_Pathfinder msg: 1730869149.28 sec; seqNo:1253; #beams:0 2024-11-06T04:59:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:59:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.02 2024-11-06T04:59:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869149.30 epoch sec; seqNo:3544; 4061533.07 north; 594846.05 east; 36.69 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T04:59:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869146.03 2024-11-06T04:59:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:09Z- waiting for 1730869150.03, time is 1730869149.30 2024-11-06T04:59:09Z- AHRS_M2 msg: 1730869149.65 epoch sec; seqNo:3327 2024-11-06T04:59:09Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 3.34 psi 2024-11-06T04:59:09Z- handleDepth()- Depth_Keller msg: 1730869149.65 epoch sec; seqNo:3545; depth 49.75 2024-11-06T04:59:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869149.66 epoch sec; seqNo:3545; 4061532.69 north; 594846.06 east; 36.69 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:59:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869146.03 2024-11-06T04:59:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:09Z- waiting for 1730869150.03, time is 1730869149.66 2024-11-06T04:59:10Z- AHRS_M2 msg: 1730869150.05 epoch sec; seqNo:3328 2024-11-06T04:59:10Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.34 psi 2024-11-06T04:59:10Z- handleDepth()- Depth_Keller msg: 1730869150.06 epoch sec; seqNo:3546; depth 49.76 2024-11-06T04:59:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869150.07 epoch sec; seqNo:3546; 4061532.31 north; 594846.06 east; 36.69 lat; -121.94 lon; 49.76 depth; utmZone: 10 2024-11-06T04:59:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869146.03 2024-11-06T04:59:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:10Z- waiting for 1730869150.03, time is 1730869150.07 2024-11-06T04:59:10Z- LcmTrn::updateTrn() >>>> pose time 1730869150.07, meas time 1730869149.28 <<<< 2024-11-06T04:59:10Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869150.07 2024-11-06T04:59:10Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:59:10Z- TerrainNav::motionUpdate() - currEstimate = (4061532, 594846.06, 49.76) 2024-11-06T04:59:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:59:10Z- TerrainNav:: SubcloudNIS is 1.02, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:59:10Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:59:10Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:59:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:59:10Z- variances([0], [2], and psi_berg) = 0.844667 0.374562 0.000000 2024-11-06T04:59:10Z- publishEstimatesDecoder() - at 1730869150.07 sec 2024-11-06T04:59:10Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:59:10Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061477.88 594850.60 60.66 2024-11-06T04:59:10Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061475.50 594850.50 61.75 lat/lon: 36.69 -121.94 2024-11-06T04:59:10Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.84 0.37 1.42 0.00 2024-11-06T04:59:10Z- AHRS_M2 msg: 1730869150.45 epoch sec; seqNo:3329 2024-11-06T04:59:10Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.34 psi 2024-11-06T04:59:10Z- handleDepth()- Depth_Keller msg: 1730869150.47 epoch sec; seqNo:3547; depth 49.73 2024-11-06T04:59:10Z- handleDVL() - DVL msg epochMillisec: 1730869150.48 2024-11-06T04:59:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:59:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:59:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:59:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:59:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:59:10Z- handleDvl() - RDI_Pathfinder msg: 1730869150.48 sec; seqNo:1254; #beams:0 2024-11-06T04:59:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:59:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , -0.02 2024-11-06T04:59:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869150.49 epoch sec; seqNo:3547; 4061531.93 north; 594846.06 east; 36.69 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T04:59:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869150.07 2024-11-06T04:59:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:10Z- waiting for 1730869154.07, time is 1730869150.49 2024-11-06T04:59:10Z- AHRS_M2 msg: 1730869150.86 epoch sec; seqNo:3330 2024-11-06T04:59:10Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.34 psi 2024-11-06T04:59:10Z- handleDepth()- Depth_Keller msg: 1730869150.87 epoch sec; seqNo:3548; depth 49.72 2024-11-06T04:59:10Z- handleDVL() - DVL msg epochMillisec: 1730869150.90 2024-11-06T04:59:10Z- handleDvl() - RDI_Pathfinder msg: 1730869150.90 sec; seqNo:1255; #beams:4 2024-11-06T04:59:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.85 , 34.01 , 41.49 , 39.06 2024-11-06T04:59:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.09 , -0.02 2024-11-06T04:59:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869150.91 epoch sec; seqNo:3548; 4061531.56 north; 594846.07 east; 36.69 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:59:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869150.07 2024-11-06T04:59:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:11Z- waiting for 1730869154.07, time is 1730869150.91 2024-11-06T04:59:11Z- AHRS_M2 msg: 1730869151.28 epoch sec; seqNo:3331 2024-11-06T04:59:11Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.34 psi 2024-11-06T04:59:11Z- handleDepth()- Depth_Keller msg: 1730869151.29 epoch sec; seqNo:3549; depth 49.73 2024-11-06T04:59:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869151.30 epoch sec; seqNo:3549; 4061531.56 north; 594846.07 east; 36.69 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T04:59:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869150.07 2024-11-06T04:59:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:11Z- waiting for 1730869154.07, time is 1730869151.30 2024-11-06T04:59:11Z- handleDepth()- Depth_Keller msg: 1730869151.69 epoch sec; seqNo:3550; depth 49.72 2024-11-06T04:59:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869151.70 epoch sec; seqNo:3550; 4061531.16 north; 594844.85 east; 36.69 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:59:11Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:59:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869150.07 2024-11-06T04:59:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:11Z- waiting for 1730869154.07, time is 1730869151.70 2024-11-06T04:59:12Z- AHRS_M2 msg: 1730869152.07 epoch sec; seqNo:3332 2024-11-06T04:59:12Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.34 psi 2024-11-06T04:59:12Z- handleDepth()- Depth_Keller msg: 1730869152.08 epoch sec; seqNo:3551; depth 49.73 2024-11-06T04:59:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869152.09 epoch sec; seqNo:3551; 4061530.78 north; 594844.86 east; 36.69 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T04:59:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869150.07 2024-11-06T04:59:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:12Z- waiting for 1730869154.07, time is 1730869152.09 2024-11-06T04:59:12Z- AHRS_M2 msg: 1730869152.48 epoch sec; seqNo:3333 2024-11-06T04:59:12Z- AHRS_M2 msg: 0.14 phi; -0.01 theta; 3.34 psi 2024-11-06T04:59:12Z- handleDepth()- Depth_Keller msg: 1730869152.48 epoch sec; seqNo:3552; depth 49.72 2024-11-06T04:59:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869152.49 epoch sec; seqNo:3552; 4061530.41 north; 594844.86 east; 36.69 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:59:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869150.07 2024-11-06T04:59:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:12Z- waiting for 1730869154.07, time is 1730869152.49 2024-11-06T04:59:12Z- AHRS_M2 msg: 1730869152.88 epoch sec; seqNo:3334 2024-11-06T04:59:12Z- AHRS_M2 msg: 0.14 phi; -0.02 theta; 3.33 psi 2024-11-06T04:59:12Z- handleDepth()- Depth_Keller msg: 1730869152.89 epoch sec; seqNo:3553; depth 49.71 2024-11-06T04:59:12Z- handleDVL() - DVL msg epochMillisec: 1730869152.91 2024-11-06T04:59:12Z- handleDvl() - RDI_Pathfinder msg: 1730869152.91 sec; seqNo:1256; #beams:4 2024-11-06T04:59:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.66 , 35.14 , 41.49 , 39.25 2024-11-06T04:59:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.12 , 0.01 2024-11-06T04:59:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869152.92 epoch sec; seqNo:3553; 4061530.03 north; 594844.87 east; 36.69 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T04:59:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869150.07 2024-11-06T04:59:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:13Z- waiting for 1730869154.07, time is 1730869152.92 2024-11-06T04:59:13Z- AHRS_M2 msg: 1730869153.28 epoch sec; seqNo:3335 2024-11-06T04:59:13Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.33 psi 2024-11-06T04:59:13Z- handleDepth()- Depth_Keller msg: 1730869153.29 epoch sec; seqNo:3554; depth 49.71 2024-11-06T04:59:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869153.31 epoch sec; seqNo:3554; 4061529.65 north; 594844.87 east; 36.69 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T04:59:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869150.07 2024-11-06T04:59:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:13Z- waiting for 1730869154.07, time is 1730869153.31 2024-11-06T04:59:13Z- AHRS_M2 msg: 1730869153.69 epoch sec; seqNo:3336 2024-11-06T04:59:13Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.33 psi 2024-11-06T04:59:13Z- handleDepth()- Depth_Keller msg: 1730869153.69 epoch sec; seqNo:3555; depth 49.72 2024-11-06T04:59:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869153.70 epoch sec; seqNo:3555; 4061529.27 north; 594844.87 east; 36.69 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T04:59:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869150.07 2024-11-06T04:59:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:13Z- waiting for 1730869154.07, time is 1730869153.70 2024-11-06T04:59:14Z- AHRS_M2 msg: 1730869154.09 epoch sec; seqNo:3337 2024-11-06T04:59:14Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.33 psi 2024-11-06T04:59:14Z- handleDepth()- Depth_Keller msg: 1730869154.11 epoch sec; seqNo:3556; depth 49.73 2024-11-06T04:59:14Z- handleDVL() - DVL msg epochMillisec: 1730869154.11 2024-11-06T04:59:14Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:14Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:14Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:14Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:59:14Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:59:14Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:59:14Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:59:14Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:59:14Z- handleDvl() - RDI_Pathfinder msg: 1730869154.11 sec; seqNo:1257; #beams:0 2024-11-06T04:59:14Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:59:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.12 , 0.01 2024-11-06T04:59:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869154.12 epoch sec; seqNo:3556; 4061528.89 north; 594844.88 east; 36.69 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T04:59:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869150.07 2024-11-06T04:59:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:14Z- waiting for 1730869154.07, time is 1730869154.12 2024-11-06T04:59:14Z- LcmTrn::updateTrn() >>>> pose time 1730869154.12, meas time 1730869154.11 <<<< 2024-11-06T04:59:14Z- LcmTrn::updateTrn() equating pose 1730869154.11 and meas times 1730869154.11 2024-11-06T04:59:14Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869154.11 2024-11-06T04:59:14Z- TerrainNav::motionUpdate() - currEstimate = (4061529, 594844.88, 49.73) 2024-11-06T04:59:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:59:14Z- TerrainNav:: SubcloudNIS is 1.02, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:59:14Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:59:14Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:59:14Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:59:14Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:59:14Z- variances([0], [2], and psi_berg) = 0.847403 0.376607 0.000000 2024-11-06T04:59:14Z- publishEstimatesDecoder() - at 1730869154.11 sec 2024-11-06T04:59:14Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:59:14Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061474.48 594849.44 60.43 2024-11-06T04:59:14Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061472.08 594849.32 61.73 lat/lon: 36.69 -121.94 2024-11-06T04:59:14Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.85 0.38 1.68 0.00 2024-11-06T04:59:14Z- AHRS_M2 msg: 1730869154.50 epoch sec; seqNo:3338 2024-11-06T04:59:14Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.33 psi 2024-11-06T04:59:14Z- handleDepth()- Depth_Keller msg: 1730869154.52 epoch sec; seqNo:3557; depth 49.71 2024-11-06T04:59:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869154.53 epoch sec; seqNo:3557; 4061528.51 north; 594844.88 east; 36.69 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T04:59:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869154.11 2024-11-06T04:59:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:14Z- waiting for 1730869158.11, time is 1730869154.53 2024-11-06T04:59:14Z- AHRS_M2 msg: 1730869154.90 epoch sec; seqNo:3339 2024-11-06T04:59:14Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.33 psi 2024-11-06T04:59:14Z- handleDepth()- Depth_Keller msg: 1730869154.91 epoch sec; seqNo:3558; depth 49.74 2024-11-06T04:59:14Z- handleDVL() - DVL msg epochMillisec: 1730869154.94 2024-11-06T04:59:14Z- handleDvl() - RDI_Pathfinder msg: 1730869154.94 sec; seqNo:1258; #beams:4 2024-11-06T04:59:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.22 , 35.14 , 41.30 , 40.00 2024-11-06T04:59:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , 0.05 2024-11-06T04:59:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869154.95 epoch sec; seqNo:3558; 4061528.13 north; 594844.89 east; 36.69 lat; -121.94 lon; 49.74 depth; utmZone: 10 2024-11-06T04:59:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869154.11 2024-11-06T04:59:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:15Z- waiting for 1730869158.11, time is 1730869154.95 2024-11-06T04:59:15Z- AHRS_M2 msg: 1730869155.31 epoch sec; seqNo:3340 2024-11-06T04:59:15Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.33 psi 2024-11-06T04:59:15Z- handleDepth()- Depth_Keller msg: 1730869155.32 epoch sec; seqNo:3559; depth 49.73 2024-11-06T04:59:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869155.34 epoch sec; seqNo:3559; 4061527.75 north; 594844.89 east; 36.69 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T04:59:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869154.11 2024-11-06T04:59:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:15Z- waiting for 1730869158.11, time is 1730869155.34 2024-11-06T04:59:15Z- AHRS_M2 msg: 1730869155.71 epoch sec; seqNo:3341 2024-11-06T04:59:15Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.33 psi 2024-11-06T04:59:15Z- handleDepth()- Depth_Keller msg: 1730869155.71 epoch sec; seqNo:3560; depth 49.75 2024-11-06T04:59:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869155.73 epoch sec; seqNo:3560; 4061527.75 north; 594844.89 east; 36.69 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:59:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869154.11 2024-11-06T04:59:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:15Z- waiting for 1730869158.11, time is 1730869155.73 2024-11-06T04:59:16Z- AHRS_M2 msg: 1730869156.11 epoch sec; seqNo:3342 2024-11-06T04:59:16Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.33 psi 2024-11-06T04:59:16Z- handleDepth()- Depth_Keller msg: 1730869156.12 epoch sec; seqNo:3561; depth 49.75 2024-11-06T04:59:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869156.14 epoch sec; seqNo:3561; 4061527.36 north; 594843.67 east; 36.69 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T04:59:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869154.11 2024-11-06T04:59:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:16Z- waiting for 1730869158.11, time is 1730869156.14 2024-11-06T04:59:16Z- AHRS_M2 msg: 1730869156.53 epoch sec; seqNo:3343 2024-11-06T04:59:16Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.33 psi 2024-11-06T04:59:16Z- handleDepth()- Depth_Keller msg: 1730869156.57 epoch sec; seqNo:3562; depth 49.77 2024-11-06T04:59:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869156.61 epoch sec; seqNo:3562; 4061526.98 north; 594843.68 east; 36.69 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T04:59:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869154.11 2024-11-06T04:59:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:16Z- waiting for 1730869158.11, time is 1730869156.61 2024-11-06T04:59:16Z- AHRS_M2 msg: 1730869156.92 epoch sec; seqNo:3344 2024-11-06T04:59:16Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.33 psi 2024-11-06T04:59:16Z- handleDepth()- Depth_Keller msg: 1730869156.93 epoch sec; seqNo:3563; depth 49.77 2024-11-06T04:59:16Z- handleDVL() - DVL msg epochMillisec: 1730869156.95 2024-11-06T04:59:16Z- handleDvl() - RDI_Pathfinder msg: 1730869156.95 sec; seqNo:1259; #beams:4 2024-11-06T04:59:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.85 , 35.14 , 41.12 , 39.81 2024-11-06T04:59:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.08 , 0.04 2024-11-06T04:59:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869156.96 epoch sec; seqNo:3563; 4061526.60 north; 594843.68 east; 36.69 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T04:59:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869154.11 2024-11-06T04:59:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:17Z- waiting for 1730869158.11, time is 1730869156.96 2024-11-06T04:59:17Z- AHRS_M2 msg: 1730869157.32 epoch sec; seqNo:3345 2024-11-06T04:59:17Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.33 psi 2024-11-06T04:59:17Z- handleDepth()- Depth_Keller msg: 1730869157.33 epoch sec; seqNo:3564; depth 49.78 2024-11-06T04:59:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869157.35 epoch sec; seqNo:3564; 4061526.23 north; 594843.69 east; 36.69 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T04:59:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869154.11 2024-11-06T04:59:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:17Z- waiting for 1730869158.11, time is 1730869157.35 2024-11-06T04:59:17Z- AHRS_M2 msg: 1730869157.73 epoch sec; seqNo:3346 2024-11-06T04:59:17Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.33 psi 2024-11-06T04:59:17Z- handleDepth()- Depth_Keller msg: 1730869157.74 epoch sec; seqNo:3565; depth 49.79 2024-11-06T04:59:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869157.75 epoch sec; seqNo:3565; 4061525.85 north; 594843.69 east; 36.69 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:59:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869154.11 2024-11-06T04:59:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:17Z- waiting for 1730869158.11, time is 1730869157.75 2024-11-06T04:59:18Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:59:18Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:59:18Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:59:18Z- AHRS_M2 msg: 1730869158.12 epoch sec; seqNo:3347 2024-11-06T04:59:18Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.33 psi 2024-11-06T04:59:18Z- handleDepth()- Depth_Keller msg: 1730869158.15 epoch sec; seqNo:3566; depth 49.79 2024-11-06T04:59:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869158.16 epoch sec; seqNo:3566; 4061525.47 north; 594843.70 east; 36.69 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T04:59:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869154.11 2024-11-06T04:59:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:18Z- waiting for 1730869158.11, time is 1730869158.16 2024-11-06T04:59:18Z- LcmTrn::updateTrn() >>>> pose time 1730869158.16, meas time 1730869156.95 <<<< 2024-11-06T04:59:18Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869158.16 2024-11-06T04:59:18Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869156.95 sec, ping # 0; waiting for INS data more recent than 1730869154.11... 2024-11-06T04:59:18Z- TerrainNav::motionUpdate() - currEstimate = (4061525, 594843.70, 49.79) 2024-11-06T04:59:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:59:18Z- TerrainNav:: SubcloudNIS is 1.02, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:59:18Z- TNavPF::Projecting Measurements 2024-11-06T04:59:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:59:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:59:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:59:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:59:18Z- TNavFilter::4 good beams. 2024-11-06T04:59:18Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:59:18Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:59:18Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:59:18Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:59:18Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:59:18Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:59:18Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:59:18Z- TerrainMapOctree: (vehN, vehE ) = (4061526.49, 594844.05). 2024-11-06T04:59:18Z- TerrainMapOctree: Min Distance = 242.49. 2024-11-06T04:59:18Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:59:18Z- Forcing Subcloud Comparison 2024-11-06T04:59:18Z- Weighting particles with subcloud comparison 2024-11-06T04:59:18Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:18Z- meanExpectedMeasDiff: 3.518540 delta_rms_squared: 5.003002 alpha: 0.996018 2024-11-06T04:59:18Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:18Z- meanExpectedMeasDiff: 0.879683 delta_rms_squared: 2.222431 alpha: 0.991081 2024-11-06T04:59:18Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:18Z- meanExpectedMeasDiff: 3.248444 delta_rms_squared: 2.554824 alpha: 0.992232 2024-11-06T04:59:18Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:18Z- meanExpectedMeasDiff: -0.473201 delta_rms_squared: 2.723581 alpha: 0.992710 2024-11-06T04:59:18Z- Subcloud NIS: 1.026376 2024-11-06T04:59:18Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:59:18Z- TNavPF:: sumWeights = 0.131274 2024-11-06T04:59:18Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:59:18Z- TNavPF::First Term of Map Covariance Matrix is 0.47 2024-11-06T04:59:18Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T04:59:18Z- TerrainNav::Resampling particle filter... 2024-11-06T04:59:18Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869156.95 sec. 2024-11-06T04:59:18Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:59:18Z- variances([0], [2], and psi_berg) = 0.779042 0.326414 0.000000 2024-11-06T04:59:18Z- publishEstimatesDecoder() - at 1730869158.16 sec 2024-11-06T04:59:18Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:59:18Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061471.02 594848.20 60.95 2024-11-06T04:59:18Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061468.67 594848.16 60.78 lat/lon: 36.69 -121.94 2024-11-06T04:59:18Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.78 0.33 0.78 0.00 2024-11-06T04:59:18Z- AHRS_M2 msg: 1730869158.54 epoch sec; seqNo:3348 2024-11-06T04:59:18Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.33 psi 2024-11-06T04:59:18Z- handleDepth()- Depth_Keller msg: 1730869158.57 epoch sec; seqNo:3567; depth 49.81 2024-11-06T04:59:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869158.61 epoch sec; seqNo:3567; 4061525.09 north; 594843.70 east; 36.69 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T04:59:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869158.16 2024-11-06T04:59:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:18Z- waiting for 1730869162.16, time is 1730869158.61 2024-11-06T04:59:18Z- AHRS_M2 msg: 1730869158.94 epoch sec; seqNo:3349 2024-11-06T04:59:18Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.33 psi 2024-11-06T04:59:18Z- handleDepth()- Depth_Keller msg: 1730869158.95 epoch sec; seqNo:3568; depth 49.83 2024-11-06T04:59:18Z- handleDVL() - DVL msg epochMillisec: 1730869158.97 2024-11-06T04:59:18Z- handleDvl() - RDI_Pathfinder msg: 1730869158.97 sec; seqNo:1260; #beams:4 2024-11-06T04:59:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.66 , 34.95 , 40.93 , 40.19 2024-11-06T04:59:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , 0.02 2024-11-06T04:59:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869158.98 epoch sec; seqNo:3568; 4061524.71 north; 594843.70 east; 36.69 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:59:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869158.16 2024-11-06T04:59:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:19Z- waiting for 1730869162.16, time is 1730869158.98 2024-11-06T04:59:19Z- AHRS_M2 msg: 1730869159.35 epoch sec; seqNo:3350 2024-11-06T04:59:19Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.33 psi 2024-11-06T04:59:19Z- handleDepth()- Depth_Keller msg: 1730869159.35 epoch sec; seqNo:3569; depth 49.82 2024-11-06T04:59:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869159.37 epoch sec; seqNo:3569; 4061524.33 north; 594843.71 east; 36.69 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T04:59:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869158.16 2024-11-06T04:59:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:19Z- waiting for 1730869162.16, time is 1730869159.37 2024-11-06T04:59:19Z- AHRS_M2 msg: 1730869159.75 epoch sec; seqNo:3351 2024-11-06T04:59:19Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 3.32 psi 2024-11-06T04:59:19Z- handleDepth()- Depth_Keller msg: 1730869159.76 epoch sec; seqNo:3570; depth 49.83 2024-11-06T04:59:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869159.77 epoch sec; seqNo:3570; 4061523.95 north; 594843.71 east; 36.69 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T04:59:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869158.16 2024-11-06T04:59:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:19Z- waiting for 1730869162.16, time is 1730869159.77 2024-11-06T04:59:20Z- AHRS_M2 msg: 1730869160.15 epoch sec; seqNo:3352 2024-11-06T04:59:20Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.32 psi 2024-11-06T04:59:20Z- handleDepth()- Depth_Keller msg: 1730869160.16 epoch sec; seqNo:3571; depth 49.85 2024-11-06T04:59:20Z- handleDVL() - DVL msg epochMillisec: 1730869160.16 2024-11-06T04:59:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:59:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:59:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:59:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:59:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:59:20Z- handleDvl() - RDI_Pathfinder msg: 1730869160.16 sec; seqNo:1261; #beams:0 2024-11-06T04:59:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:59:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.06 , 0.02 2024-11-06T04:59:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869160.17 epoch sec; seqNo:3571; 4061523.95 north; 594843.71 east; 36.69 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:59:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869158.16 2024-11-06T04:59:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:20Z- waiting for 1730869162.16, time is 1730869160.17 2024-11-06T04:59:20Z- AHRS_M2 msg: 1730869160.57 epoch sec; seqNo:3353 2024-11-06T04:59:20Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.32 psi 2024-11-06T04:59:20Z- handleDepth()- Depth_Keller msg: 1730869160.58 epoch sec; seqNo:3572; depth 49.85 2024-11-06T04:59:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869160.59 epoch sec; seqNo:3572; 4061523.57 north; 594843.72 east; 36.69 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T04:59:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869158.16 2024-11-06T04:59:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:20Z- waiting for 1730869162.16, time is 1730869160.59 2024-11-06T04:59:20Z- AHRS_M2 msg: 1730869160.96 epoch sec; seqNo:3354 2024-11-06T04:59:20Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.32 psi 2024-11-06T04:59:20Z- handleDepth()- Depth_Keller msg: 1730869160.96 epoch sec; seqNo:3573; depth 49.87 2024-11-06T04:59:20Z- handleDVL() - DVL msg epochMillisec: 1730869160.99 2024-11-06T04:59:20Z- handleDvl() - RDI_Pathfinder msg: 1730869160.99 sec; seqNo:1262; #beams:4 2024-11-06T04:59:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.85 , 34.76 , 40.56 , 40.18 2024-11-06T04:59:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.05 , 0.01 2024-11-06T04:59:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869161.02 epoch sec; seqNo:3573; 4061523.19 north; 594843.72 east; 36.69 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T04:59:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869158.16 2024-11-06T04:59:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:21Z- waiting for 1730869162.16, time is 1730869161.02 2024-11-06T04:59:21Z- AHRS_M2 msg: 1730869161.37 epoch sec; seqNo:3355 2024-11-06T04:59:21Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.32 psi 2024-11-06T04:59:21Z- handleDepth()- Depth_Keller msg: 1730869161.38 epoch sec; seqNo:3574; depth 49.88 2024-11-06T04:59:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869161.39 epoch sec; seqNo:3574; 4061522.80 north; 594842.50 east; 36.69 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T04:59:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869158.16 2024-11-06T04:59:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:21Z- waiting for 1730869162.16, time is 1730869161.39 2024-11-06T04:59:21Z- AHRS_M2 msg: 1730869161.77 epoch sec; seqNo:3356 2024-11-06T04:59:21Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.32 psi 2024-11-06T04:59:21Z- handleDepth()- Depth_Keller msg: 1730869161.79 epoch sec; seqNo:3575; depth 49.89 2024-11-06T04:59:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869161.80 epoch sec; seqNo:3575; 4061522.42 north; 594842.51 east; 36.69 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:59:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869158.16 2024-11-06T04:59:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:21Z- waiting for 1730869162.16, time is 1730869161.80 2024-11-06T04:59:22Z- AHRS_M2 msg: 1730869162.17 epoch sec; seqNo:3357 2024-11-06T04:59:22Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.32 psi 2024-11-06T04:59:22Z- handleDepth()- Depth_Keller msg: 1730869162.17 epoch sec; seqNo:3576; depth 49.89 2024-11-06T04:59:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869162.18 epoch sec; seqNo:3576; 4061522.04 north; 594842.51 east; 36.69 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T04:59:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869158.16 2024-11-06T04:59:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:22Z- waiting for 1730869162.16, time is 1730869162.18 2024-11-06T04:59:22Z- LcmTrn::updateTrn() >>>> pose time 1730869162.18, meas time 1730869160.99 <<<< 2024-11-06T04:59:22Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869162.18 2024-11-06T04:59:22Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869160.99 sec, ping # 0; waiting for INS data more recent than 1730869158.16... 2024-11-06T04:59:22Z- TerrainNav::motionUpdate() - currEstimate = (4061522, 594842.51, 49.89) 2024-11-06T04:59:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:59:22Z- TerrainNav:: SubcloudNIS is 1.03, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:59:22Z- TNavPF::Projecting Measurements 2024-11-06T04:59:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:59:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:59:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:59:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:59:22Z- TNavFilter::4 good beams. 2024-11-06T04:59:22Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:59:22Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:59:22Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:59:22Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:59:22Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:59:22Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:59:22Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:59:22Z- TerrainMapOctree: (vehN, vehE ) = (4061523.06, 594842.86). 2024-11-06T04:59:22Z- TerrainMapOctree: Min Distance = 245.20. 2024-11-06T04:59:22Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:59:22Z- Forcing Subcloud Comparison 2024-11-06T04:59:22Z- Weighting particles with subcloud comparison 2024-11-06T04:59:22Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:22Z- meanExpectedMeasDiff: 2.998301 delta_rms_squared: 2.029404 alpha: 0.990241 2024-11-06T04:59:22Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:22Z- meanExpectedMeasDiff: -0.462395 delta_rms_squared: 1.147687 alpha: 0.982872 2024-11-06T04:59:22Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:22Z- meanExpectedMeasDiff: 1.846399 delta_rms_squared: 1.306608 alpha: 0.984924 2024-11-06T04:59:22Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:22Z- meanExpectedMeasDiff: -1.476369 delta_rms_squared: 1.431576 alpha: 0.986222 2024-11-06T04:59:22Z- Subcloud NIS: 0.995701 2024-11-06T04:59:22Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:59:22Z- TNavPF:: sumWeights = 0.162309 2024-11-06T04:59:22Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:59:22Z- TNavPF::First Term of Map Covariance Matrix is 0.93 2024-11-06T04:59:22Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T04:59:22Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869160.99 sec. 2024-11-06T04:59:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:59:22Z- variances([0], [2], and psi_berg) = 0.811852 0.331342 0.000000 2024-11-06T04:59:22Z- publishEstimatesDecoder() - at 1730869162.18 sec 2024-11-06T04:59:22Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:59:22Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061466.73 594847.49 60.71 2024-11-06T04:59:22Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061465.27 594846.97 60.58 lat/lon: 36.69 -121.94 2024-11-06T04:59:22Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.81 0.33 0.70 0.00 2024-11-06T04:59:22Z- AHRS_M2 msg: 1730869162.58 epoch sec; seqNo:3358 2024-11-06T04:59:22Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.32 psi 2024-11-06T04:59:22Z- handleDepth()- Depth_Keller msg: 1730869162.60 epoch sec; seqNo:3577; depth 49.92 2024-11-06T04:59:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869162.64 epoch sec; seqNo:3577; 4061521.67 north; 594842.52 east; 36.69 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:59:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869162.18 2024-11-06T04:59:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:22Z- waiting for 1730869166.18, time is 1730869162.64 2024-11-06T04:59:22Z- AHRS_M2 msg: 1730869162.98 epoch sec; seqNo:3359 2024-11-06T04:59:22Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.32 psi 2024-11-06T04:59:22Z- handleDepth()- Depth_Keller msg: 1730869162.98 epoch sec; seqNo:3578; depth 49.92 2024-11-06T04:59:23Z- handleDVL() - DVL msg epochMillisec: 1730869163.00 2024-11-06T04:59:23Z- handleDvl() - RDI_Pathfinder msg: 1730869163.00 sec; seqNo:1263; #beams:4 2024-11-06T04:59:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.66 , 34.58 , 40.74 , 40.37 2024-11-06T04:59:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , -0.01 2024-11-06T04:59:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869163.03 epoch sec; seqNo:3578; 4061521.29 north; 594842.52 east; 36.69 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T04:59:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869162.18 2024-11-06T04:59:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:23Z- waiting for 1730869166.18, time is 1730869163.03 2024-11-06T04:59:23Z- AHRS_M2 msg: 1730869163.39 epoch sec; seqNo:3360 2024-11-06T04:59:23Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.32 psi 2024-11-06T04:59:23Z- handleDepth()- Depth_Keller msg: 1730869163.39 epoch sec; seqNo:3579; depth 49.93 2024-11-06T04:59:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869163.41 epoch sec; seqNo:3579; 4061520.91 north; 594842.53 east; 36.69 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T04:59:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869162.18 2024-11-06T04:59:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:23Z- waiting for 1730869166.18, time is 1730869163.41 2024-11-06T04:59:23Z- AHRS_M2 msg: 1730869163.79 epoch sec; seqNo:3361 2024-11-06T04:59:23Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.32 psi 2024-11-06T04:59:23Z- handleDepth()- Depth_Keller msg: 1730869163.81 epoch sec; seqNo:3580; depth 49.93 2024-11-06T04:59:23Z- handleDVL() - DVL msg epochMillisec: 1730869163.81 2024-11-06T04:59:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:59:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:59:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:59:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:59:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:59:23Z- handleDvl() - RDI_Pathfinder msg: 1730869163.81 sec; seqNo:1264; #beams:0 2024-11-06T04:59:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:59:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , -0.01 2024-11-06T04:59:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869163.82 epoch sec; seqNo:3580; 4061520.53 north; 594842.53 east; 36.69 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T04:59:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869162.18 2024-11-06T04:59:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:23Z- waiting for 1730869166.18, time is 1730869163.82 2024-11-06T04:59:24Z- AHRS_M2 msg: 1730869164.19 epoch sec; seqNo:3362 2024-11-06T04:59:24Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.32 psi 2024-11-06T04:59:24Z- handleDepth()- Depth_Keller msg: 1730869164.19 epoch sec; seqNo:3581; depth 49.95 2024-11-06T04:59:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869164.20 epoch sec; seqNo:3581; 4061520.15 north; 594842.53 east; 36.69 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T04:59:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869162.18 2024-11-06T04:59:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:24Z- waiting for 1730869166.18, time is 1730869164.20 2024-11-06T04:59:24Z- AHRS_M2 msg: 1730869164.60 epoch sec; seqNo:3363 2024-11-06T04:59:24Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.32 psi 2024-11-06T04:59:24Z- handleDepth()- Depth_Keller msg: 1730869164.60 epoch sec; seqNo:3582; depth 49.95 2024-11-06T04:59:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869164.61 epoch sec; seqNo:3582; 4061519.77 north; 594842.54 east; 36.69 lat; -121.94 lon; 49.95 depth; utmZone: 10 2024-11-06T04:59:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869162.18 2024-11-06T04:59:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:24Z- waiting for 1730869166.18, time is 1730869164.61 2024-11-06T04:59:25Z- AHRS_M2 msg: 1730869165.00 epoch sec; seqNo:3364 2024-11-06T04:59:25Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.32 psi 2024-11-06T04:59:25Z- handleDepth()- Depth_Keller msg: 1730869165.01 epoch sec; seqNo:3583; depth 49.96 2024-11-06T04:59:25Z- handleDVL() - DVL msg epochMillisec: 1730869165.03 2024-11-06T04:59:25Z- handleDvl() - RDI_Pathfinder msg: 1730869165.03 sec; seqNo:1265; #beams:4 2024-11-06T04:59:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.22 , 34.39 , 40.37 , 41.12 2024-11-06T04:59:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.06 , 0.01 2024-11-06T04:59:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869165.05 epoch sec; seqNo:3583; 4061519.39 north; 594842.54 east; 36.69 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T04:59:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869162.18 2024-11-06T04:59:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:25Z- waiting for 1730869166.18, time is 1730869165.05 2024-11-06T04:59:25Z- AHRS_M2 msg: 1730869165.40 epoch sec; seqNo:3365 2024-11-06T04:59:25Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:25Z- handleDepth()- Depth_Keller msg: 1730869165.40 epoch sec; seqNo:3584; depth 49.97 2024-11-06T04:59:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869165.43 epoch sec; seqNo:3584; 4061519.01 north; 594842.55 east; 36.69 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T04:59:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869162.18 2024-11-06T04:59:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:25Z- waiting for 1730869166.18, time is 1730869165.43 2024-11-06T04:59:25Z- handleDepth()- Depth_Keller msg: 1730869165.83 epoch sec; seqNo:3585; depth 49.98 2024-11-06T04:59:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869165.85 epoch sec; seqNo:3585; 4061518.63 north; 594842.55 east; 36.69 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T04:59:25Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:59:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869162.18 2024-11-06T04:59:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:25Z- waiting for 1730869166.18, time is 1730869165.85 2024-11-06T04:59:26Z- AHRS_M2 msg: 1730869166.21 epoch sec; seqNo:3366 2024-11-06T04:59:26Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:26Z- handleDepth()- Depth_Keller msg: 1730869166.22 epoch sec; seqNo:3586; depth 49.99 2024-11-06T04:59:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869166.22 epoch sec; seqNo:3586; 4061518.26 north; 594842.55 east; 36.69 lat; -121.94 lon; 49.99 depth; utmZone: 10 2024-11-06T04:59:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869162.18 2024-11-06T04:59:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:26Z- waiting for 1730869166.18, time is 1730869166.22 2024-11-06T04:59:26Z- LcmTrn::updateTrn() >>>> pose time 1730869166.22, meas time 1730869165.03 <<<< 2024-11-06T04:59:26Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869166.22 2024-11-06T04:59:26Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869165.03 sec, ping # 0; waiting for INS data more recent than 1730869162.18... 2024-11-06T04:59:26Z- TerrainNav::motionUpdate() - currEstimate = (4061518, 594842.55, 49.99) 2024-11-06T04:59:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:59:26Z- TerrainNav:: SubcloudNIS is 1.00, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:59:26Z- TNavPF::Projecting Measurements 2024-11-06T04:59:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:59:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:59:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:59:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:59:26Z- TNavFilter::4 good beams. 2024-11-06T04:59:26Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:59:26Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:59:26Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:59:26Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:59:26Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:59:26Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:59:26Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:59:26Z- TerrainMapOctree: (vehN, vehE ) = (4061519.38, 594842.54). 2024-11-06T04:59:26Z- TerrainMapOctree: Min Distance = 248.50. 2024-11-06T04:59:26Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:59:26Z- Forcing Subcloud Comparison 2024-11-06T04:59:26Z- Weighting particles with subcloud comparison 2024-11-06T04:59:26Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:26Z- meanExpectedMeasDiff: 2.875813 delta_rms_squared: 2.053880 alpha: 0.990356 2024-11-06T04:59:26Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:26Z- meanExpectedMeasDiff: -0.970386 delta_rms_squared: 1.084803 alpha: 0.981897 2024-11-06T04:59:26Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:26Z- meanExpectedMeasDiff: 1.450717 delta_rms_squared: 1.239539 alpha: 0.984121 2024-11-06T04:59:26Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:26Z- meanExpectedMeasDiff: -0.888436 delta_rms_squared: 1.372066 alpha: 0.985633 2024-11-06T04:59:26Z- Subcloud NIS: 0.977375 2024-11-06T04:59:26Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:59:26Z- TNavPF:: sumWeights = 0.208544 2024-11-06T04:59:26Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:59:26Z- TNavPF::First Term of Map Covariance Matrix is 1.08 2024-11-06T04:59:26Z- TNavPF::Calculated NIS Value : 0.11 numBeams normalized NIS: 0.03 2024-11-06T04:59:26Z- TerrainNav::Resampling particle filter... 2024-11-06T04:59:26Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869165.03 sec. 2024-11-06T04:59:26Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:59:26Z- variances([0], [2], and psi_berg) = 0.840362 0.342157 0.000000 2024-11-06T04:59:26Z- publishEstimatesDecoder() - at 1730869166.22 sec 2024-11-06T04:59:26Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:59:26Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061463.87 594847.05 61.05 2024-11-06T04:59:26Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061461.46 594847.02 60.44 lat/lon: 36.69 -121.94 2024-11-06T04:59:26Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.84 0.34 0.69 0.00 2024-11-06T04:59:26Z- AHRS_M2 msg: 1730869166.62 epoch sec; seqNo:3367 2024-11-06T04:59:26Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:26Z- handleDepth()- Depth_Keller msg: 1730869166.63 epoch sec; seqNo:3587; depth 50.01 2024-11-06T04:59:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869166.64 epoch sec; seqNo:3587; 4061517.88 north; 594842.56 east; 36.69 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:59:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869166.22 2024-11-06T04:59:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:26Z- waiting for 1730869170.22, time is 1730869166.64 2024-11-06T04:59:27Z- AHRS_M2 msg: 1730869167.02 epoch sec; seqNo:3368 2024-11-06T04:59:27Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:27Z- handleDepth()- Depth_Keller msg: 1730869167.03 epoch sec; seqNo:3588; depth 50.00 2024-11-06T04:59:27Z- handleDVL() - DVL msg epochMillisec: 1730869167.05 2024-11-06T04:59:27Z- handleDvl() - RDI_Pathfinder msg: 1730869167.05 sec; seqNo:1266; #beams:4 2024-11-06T04:59:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.85 , 34.39 , 40.56 , 41.12 2024-11-06T04:59:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.06 , 0.02 2024-11-06T04:59:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869167.07 epoch sec; seqNo:3588; 4061517.48 north; 594841.34 east; 36.69 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T04:59:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869166.22 2024-11-06T04:59:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:27Z- waiting for 1730869170.22, time is 1730869167.07 2024-11-06T04:59:27Z- AHRS_M2 msg: 1730869167.42 epoch sec; seqNo:3369 2024-11-06T04:59:27Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:27Z- handleDepth()- Depth_Keller msg: 1730869167.43 epoch sec; seqNo:3589; depth 50.01 2024-11-06T04:59:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869167.44 epoch sec; seqNo:3589; 4061517.48 north; 594841.34 east; 36.69 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T04:59:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869166.22 2024-11-06T04:59:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:27Z- waiting for 1730869170.22, time is 1730869167.44 2024-11-06T04:59:27Z- AHRS_M2 msg: 1730869167.83 epoch sec; seqNo:3370 2024-11-06T04:59:27Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:27Z- handleDepth()- Depth_Keller msg: 1730869167.85 epoch sec; seqNo:3590; depth 50.02 2024-11-06T04:59:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869167.85 epoch sec; seqNo:3590; 4061517.11 north; 594841.35 east; 36.69 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T04:59:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869166.22 2024-11-06T04:59:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:27Z- waiting for 1730869170.22, time is 1730869167.85 2024-11-06T04:59:28Z- AHRS_M2 msg: 1730869168.23 epoch sec; seqNo:3371 2024-11-06T04:59:28Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:28Z- handleDepth()- Depth_Keller msg: 1730869168.24 epoch sec; seqNo:3591; depth 50.05 2024-11-06T04:59:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869168.25 epoch sec; seqNo:3591; 4061516.73 north; 594841.35 east; 36.69 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T04:59:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869166.22 2024-11-06T04:59:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:28Z- waiting for 1730869170.22, time is 1730869168.25 2024-11-06T04:59:28Z- AHRS_M2 msg: 1730869168.63 epoch sec; seqNo:3372 2024-11-06T04:59:28Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:28Z- handleDepth()- Depth_Keller msg: 1730869168.65 epoch sec; seqNo:3592; depth 50.05 2024-11-06T04:59:28Z- handleDVL() - DVL msg epochMillisec: 1730869168.66 2024-11-06T04:59:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:59:28Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:59:28Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:59:28Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:59:28Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:59:28Z- handleDvl() - RDI_Pathfinder msg: 1730869168.66 sec; seqNo:1267; #beams:0 2024-11-06T04:59:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:59:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.06 , 0.02 2024-11-06T04:59:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869168.68 epoch sec; seqNo:3592; 4061516.35 north; 594841.36 east; 36.69 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T04:59:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869166.22 2024-11-06T04:59:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:28Z- waiting for 1730869170.22, time is 1730869168.68 2024-11-06T04:59:29Z- AHRS_M2 msg: 1730869169.04 epoch sec; seqNo:3373 2024-11-06T04:59:29Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:29Z- handleDepth()- Depth_Keller msg: 1730869169.04 epoch sec; seqNo:3593; depth 50.07 2024-11-06T04:59:29Z- handleDVL() - DVL msg epochMillisec: 1730869169.07 2024-11-06T04:59:29Z- handleDvl() - RDI_Pathfinder msg: 1730869169.07 sec; seqNo:1268; #beams:4 2024-11-06T04:59:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.22 , 34.21 , 40.19 , 41.12 2024-11-06T04:59:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.07 , 0.01 2024-11-06T04:59:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869169.09 epoch sec; seqNo:3593; 4061515.97 north; 594841.36 east; 36.69 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T04:59:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869166.22 2024-11-06T04:59:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:29Z- waiting for 1730869170.22, time is 1730869169.09 2024-11-06T04:59:29Z- AHRS_M2 msg: 1730869169.44 epoch sec; seqNo:3374 2024-11-06T04:59:29Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:29Z- handleDepth()- Depth_Keller msg: 1730869169.44 epoch sec; seqNo:3594; depth 50.09 2024-11-06T04:59:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869169.47 epoch sec; seqNo:3594; 4061515.59 north; 594841.36 east; 36.69 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T04:59:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869166.22 2024-11-06T04:59:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:29Z- waiting for 1730869170.22, time is 1730869169.47 2024-11-06T04:59:29Z- AHRS_M2 msg: 1730869169.84 epoch sec; seqNo:3375 2024-11-06T04:59:29Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:29Z- handleDepth()- Depth_Keller msg: 1730869169.85 epoch sec; seqNo:3595; depth 50.08 2024-11-06T04:59:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869169.87 epoch sec; seqNo:3595; 4061515.21 north; 594841.37 east; 36.69 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T04:59:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869166.22 2024-11-06T04:59:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:30Z- waiting for 1730869170.22, time is 1730869169.87 2024-11-06T04:59:30Z- AHRS_M2 msg: 1730869170.25 epoch sec; seqNo:3376 2024-11-06T04:59:30Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:30Z- handleDepth()- Depth_Keller msg: 1730869170.25 epoch sec; seqNo:3596; depth 50.10 2024-11-06T04:59:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869170.26 epoch sec; seqNo:3596; 4061514.83 north; 594841.37 east; 36.69 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T04:59:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869166.22 2024-11-06T04:59:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:30Z- waiting for 1730869170.22, time is 1730869170.26 2024-11-06T04:59:30Z- LcmTrn::updateTrn() >>>> pose time 1730869170.26, meas time 1730869169.07 <<<< 2024-11-06T04:59:30Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869170.26 2024-11-06T04:59:30Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869169.07 sec, ping # 0; waiting for INS data more recent than 1730869166.22... 2024-11-06T04:59:30Z- TerrainNav::motionUpdate() - currEstimate = (4061515, 594841.37, 50.10) 2024-11-06T04:59:30Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:59:30Z- TerrainNav:: SubcloudNIS is 0.98, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:59:30Z- TNavPF::Projecting Measurements 2024-11-06T04:59:30Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:59:30Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:59:30Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:59:30Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:59:30Z- TNavFilter::4 good beams. 2024-11-06T04:59:30Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:59:30Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:59:30Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:59:30Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:59:30Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:59:30Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:59:30Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:59:30Z- TerrainMapOctree: (vehN, vehE ) = (4061515.84, 594841.72). 2024-11-06T04:59:30Z- TerrainMapOctree: Min Distance = 251.48. 2024-11-06T04:59:30Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:59:30Z- Forcing Subcloud Comparison 2024-11-06T04:59:30Z- Weighting particles with subcloud comparison 2024-11-06T04:59:30Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:30Z- meanExpectedMeasDiff: 2.717255 delta_rms_squared: 1.742892 alpha: 0.988655 2024-11-06T04:59:30Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:30Z- meanExpectedMeasDiff: -1.270818 delta_rms_squared: 1.068010 alpha: 0.981618 2024-11-06T04:59:30Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:30Z- meanExpectedMeasDiff: 1.062238 delta_rms_squared: 1.225536 alpha: 0.983943 2024-11-06T04:59:30Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:30Z- meanExpectedMeasDiff: -0.930669 delta_rms_squared: 1.533033 alpha: 0.987122 2024-11-06T04:59:30Z- Subcloud NIS: 0.945701 2024-11-06T04:59:30Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:59:30Z- TNavPF:: sumWeights = 0.221124 2024-11-06T04:59:30Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:59:30Z- TNavPF::First Term of Map Covariance Matrix is 1.05 2024-11-06T04:59:30Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T04:59:30Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869169.07 sec. 2024-11-06T04:59:30Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:59:30Z- variances([0], [2], and psi_berg) = 0.853519 0.353191 0.000000 2024-11-06T04:59:30Z- publishEstimatesDecoder() - at 1730869170.26 sec 2024-11-06T04:59:30Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:59:30Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061458.97 594847.10 60.53 2024-11-06T04:59:30Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061458.04 594845.84 60.43 lat/lon: 36.69 -121.94 2024-11-06T04:59:30Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.85 0.35 0.68 0.00 2024-11-06T04:59:30Z- AHRS_M2 msg: 1730869170.65 epoch sec; seqNo:3377 2024-11-06T04:59:30Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:30Z- handleDepth()- Depth_Keller msg: 1730869170.67 epoch sec; seqNo:3597; depth 50.11 2024-11-06T04:59:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869170.72 epoch sec; seqNo:3597; 4061514.45 north; 594841.38 east; 36.69 lat; -121.94 lon; 50.11 depth; utmZone: 10 2024-11-06T04:59:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869170.26 2024-11-06T04:59:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:30Z- waiting for 1730869174.26, time is 1730869170.72 2024-11-06T04:59:31Z- AHRS_M2 msg: 1730869171.06 epoch sec; seqNo:3378 2024-11-06T04:59:31Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:31Z- handleDepth()- Depth_Keller msg: 1730869171.06 epoch sec; seqNo:3598; depth 50.13 2024-11-06T04:59:31Z- handleDVL() - DVL msg epochMillisec: 1730869171.08 2024-11-06T04:59:31Z- handleDvl() - RDI_Pathfinder msg: 1730869171.08 sec; seqNo:1269; #beams:4 2024-11-06T04:59:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.85 , 34.02 , 40.19 , 40.93 2024-11-06T04:59:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , 0.00 2024-11-06T04:59:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869171.09 epoch sec; seqNo:3598; 4061514.08 north; 594841.38 east; 36.69 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T04:59:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869170.26 2024-11-06T04:59:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:31Z- waiting for 1730869174.26, time is 1730869171.09 2024-11-06T04:59:31Z- AHRS_M2 msg: 1730869171.46 epoch sec; seqNo:3379 2024-11-06T04:59:31Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:31Z- handleDepth()- Depth_Keller msg: 1730869171.47 epoch sec; seqNo:3599; depth 50.14 2024-11-06T04:59:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869171.49 epoch sec; seqNo:3599; 4061513.70 north; 594841.38 east; 36.69 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T04:59:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869170.26 2024-11-06T04:59:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:31Z- waiting for 1730869174.26, time is 1730869171.49 2024-11-06T04:59:31Z- AHRS_M2 msg: 1730869171.87 epoch sec; seqNo:3380 2024-11-06T04:59:31Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:31Z- handleDepth()- Depth_Keller msg: 1730869171.87 epoch sec; seqNo:3600; depth 50.17 2024-11-06T04:59:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869171.88 epoch sec; seqNo:3600; 4061513.30 north; 594840.17 east; 36.69 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T04:59:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869170.26 2024-11-06T04:59:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:32Z- waiting for 1730869174.26, time is 1730869171.88 2024-11-06T04:59:32Z- AHRS_M2 msg: 1730869172.27 epoch sec; seqNo:3381 2024-11-06T04:59:32Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:32Z- handleDepth()- Depth_Keller msg: 1730869172.28 epoch sec; seqNo:3601; depth 50.17 2024-11-06T04:59:32Z- handleDVL() - DVL msg epochMillisec: 1730869172.28 2024-11-06T04:59:32Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:32Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:32Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:32Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:59:32Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:59:32Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:59:32Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:59:32Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:59:32Z- handleDvl() - RDI_Pathfinder msg: 1730869172.28 sec; seqNo:1270; #beams:0 2024-11-06T04:59:32Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:59:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.10 , 0.00 2024-11-06T04:59:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869172.29 epoch sec; seqNo:3601; 4061512.92 north; 594840.17 east; 36.69 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T04:59:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869170.26 2024-11-06T04:59:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:32Z- waiting for 1730869174.26, time is 1730869172.29 2024-11-06T04:59:32Z- AHRS_M2 msg: 1730869172.68 epoch sec; seqNo:3382 2024-11-06T04:59:32Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:32Z- handleDepth()- Depth_Keller msg: 1730869172.70 epoch sec; seqNo:3602; depth 50.18 2024-11-06T04:59:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869172.76 epoch sec; seqNo:3602; 4061512.55 north; 594840.18 east; 36.69 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T04:59:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869170.26 2024-11-06T04:59:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:32Z- waiting for 1730869174.26, time is 1730869172.76 2024-11-06T04:59:33Z- AHRS_M2 msg: 1730869173.08 epoch sec; seqNo:3383 2024-11-06T04:59:33Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:33Z- handleDepth()- Depth_Keller msg: 1730869173.08 epoch sec; seqNo:3603; depth 50.20 2024-11-06T04:59:33Z- handleDVL() - DVL msg epochMillisec: 1730869173.11 2024-11-06T04:59:33Z- handleDvl() - RDI_Pathfinder msg: 1730869173.11 sec; seqNo:1271; #beams:4 2024-11-06T04:59:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.85 , 33.83 , 40.00 , 40.56 2024-11-06T04:59:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.09 , 0.04 2024-11-06T04:59:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869173.12 epoch sec; seqNo:3603; 4061512.55 north; 594840.18 east; 36.69 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T04:59:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869170.26 2024-11-06T04:59:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:33Z- waiting for 1730869174.26, time is 1730869173.12 2024-11-06T04:59:33Z- AHRS_M2 msg: 1730869173.48 epoch sec; seqNo:3384 2024-11-06T04:59:33Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:33Z- handleDepth()- Depth_Keller msg: 1730869173.49 epoch sec; seqNo:3604; depth 50.21 2024-11-06T04:59:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869173.50 epoch sec; seqNo:3604; 4061512.17 north; 594840.18 east; 36.69 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T04:59:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869170.26 2024-11-06T04:59:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:33Z- waiting for 1730869174.26, time is 1730869173.50 2024-11-06T04:59:33Z- AHRS_M2 msg: 1730869173.88 epoch sec; seqNo:3385 2024-11-06T04:59:33Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:33Z- handleDepth()- Depth_Keller msg: 1730869173.89 epoch sec; seqNo:3605; depth 50.21 2024-11-06T04:59:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869173.90 epoch sec; seqNo:3605; 4061511.79 north; 594840.19 east; 36.69 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T04:59:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869170.26 2024-11-06T04:59:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:34Z- waiting for 1730869174.26, time is 1730869173.90 2024-11-06T04:59:34Z- AHRS_M2 msg: 1730869174.29 epoch sec; seqNo:3386 2024-11-06T04:59:34Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:34Z- handleDepth()- Depth_Keller msg: 1730869174.29 epoch sec; seqNo:3606; depth 50.25 2024-11-06T04:59:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869174.30 epoch sec; seqNo:3606; 4061511.41 north; 594840.19 east; 36.69 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T04:59:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869170.26 2024-11-06T04:59:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:34Z- waiting for 1730869174.26, time is 1730869174.30 2024-11-06T04:59:34Z- LcmTrn::updateTrn() >>>> pose time 1730869174.30, meas time 1730869173.11 <<<< 2024-11-06T04:59:34Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869174.30 2024-11-06T04:59:34Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869173.11 sec, ping # 0; waiting for INS data more recent than 1730869170.26... 2024-11-06T04:59:34Z- TerrainNav::motionUpdate() - currEstimate = (4061511, 594840.19, 50.25) 2024-11-06T04:59:34Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:59:34Z- TerrainNav:: SubcloudNIS is 0.95, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:59:34Z- TNavPF::Projecting Measurements 2024-11-06T04:59:34Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:59:34Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:59:34Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:59:34Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:59:34Z- TNavFilter::4 good beams. 2024-11-06T04:59:34Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:59:34Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:59:34Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:59:34Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:59:34Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:59:34Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:59:34Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:59:34Z- TerrainMapOctree: (vehN, vehE ) = (4061512.42, 594840.54). 2024-11-06T04:59:34Z- TerrainMapOctree: Min Distance = 254.24. 2024-11-06T04:59:34Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:59:34Z- Forcing Subcloud Comparison 2024-11-06T04:59:34Z- Weighting particles with subcloud comparison 2024-11-06T04:59:34Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:34Z- meanExpectedMeasDiff: 2.507263 delta_rms_squared: 1.552985 alpha: 0.987285 2024-11-06T04:59:34Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:34Z- meanExpectedMeasDiff: -1.658150 delta_rms_squared: 1.066597 alpha: 0.981594 2024-11-06T04:59:34Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:34Z- meanExpectedMeasDiff: 0.918143 delta_rms_squared: 1.220172 alpha: 0.983873 2024-11-06T04:59:34Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:34Z- meanExpectedMeasDiff: -0.659657 delta_rms_squared: 2.042820 alpha: 0.990305 2024-11-06T04:59:34Z- Subcloud NIS: 0.927519 2024-11-06T04:59:34Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:59:34Z- TNavPF:: sumWeights = 0.217457 2024-11-06T04:59:34Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:59:34Z- TNavPF::First Term of Map Covariance Matrix is 0.94 2024-11-06T04:59:34Z- TNavPF::Calculated NIS Value : 0.12 numBeams normalized NIS: 0.03 2024-11-06T04:59:34Z- TerrainNav::Resampling particle filter... 2024-11-06T04:59:34Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869173.11 sec. 2024-11-06T04:59:34Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:59:34Z- variances([0], [2], and psi_berg) = 0.862828 0.342503 0.000000 2024-11-06T04:59:34Z- publishEstimatesDecoder() - at 1730869174.30 sec 2024-11-06T04:59:34Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:59:34Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061457.02 594844.64 61.70 2024-11-06T04:59:34Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061454.63 594844.60 60.53 lat/lon: 36.69 -121.94 2024-11-06T04:59:34Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.86 0.34 0.68 0.00 2024-11-06T04:59:34Z- AHRS_M2 msg: 1730869174.76 epoch sec; seqNo:3387 2024-11-06T04:59:34Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:34Z- handleDepth()- Depth_Keller msg: 1730869174.77 epoch sec; seqNo:3607; depth 50.25 2024-11-06T04:59:34Z- handleDVL() - DVL msg epochMillisec: 1730869174.80 2024-11-06T04:59:34Z- handleDvl() - RDI_Pathfinder msg: 1730869174.80 sec; seqNo:1272; #beams:4 2024-11-06T04:59:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.48 , 33.27 , 40.00 , 41.12 2024-11-06T04:59:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.02 , 0.06 2024-11-06T04:59:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869174.84 epoch sec; seqNo:3607; 4061511.03 north; 594840.19 east; 36.69 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T04:59:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869174.30 2024-11-06T04:59:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:34Z- waiting for 1730869178.30, time is 1730869174.84 2024-11-06T04:59:35Z- AHRS_M2 msg: 1730869175.10 epoch sec; seqNo:3388 2024-11-06T04:59:35Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:35Z- handleDepth()- Depth_Keller msg: 1730869175.10 epoch sec; seqNo:3608; depth 50.27 2024-11-06T04:59:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869175.12 epoch sec; seqNo:3608; 4061510.65 north; 594840.20 east; 36.69 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T04:59:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869174.30 2024-11-06T04:59:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:35Z- waiting for 1730869178.30, time is 1730869175.12 2024-11-06T04:59:35Z- AHRS_M2 msg: 1730869175.50 epoch sec; seqNo:3389 2024-11-06T04:59:35Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:35Z- handleDepth()- Depth_Keller msg: 1730869175.51 epoch sec; seqNo:3609; depth 50.26 2024-11-06T04:59:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869175.52 epoch sec; seqNo:3609; 4061510.27 north; 594840.20 east; 36.69 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T04:59:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869174.30 2024-11-06T04:59:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:35Z- waiting for 1730869178.30, time is 1730869175.52 2024-11-06T04:59:35Z- AHRS_M2 msg: 1730869175.90 epoch sec; seqNo:3390 2024-11-06T04:59:35Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.33 psi 2024-11-06T04:59:35Z- handleDepth()- Depth_Keller msg: 1730869175.91 epoch sec; seqNo:3610; depth 50.28 2024-11-06T04:59:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869175.93 epoch sec; seqNo:3610; 4061509.89 north; 594840.21 east; 36.69 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T04:59:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869174.30 2024-11-06T04:59:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:36Z- waiting for 1730869178.30, time is 1730869175.93 2024-11-06T04:59:36Z- AHRS_M2 msg: 1730869176.31 epoch sec; seqNo:3391 2024-11-06T04:59:36Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.34 psi 2024-11-06T04:59:36Z- handleDepth()- Depth_Keller msg: 1730869176.31 epoch sec; seqNo:3611; depth 50.30 2024-11-06T04:59:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869176.33 epoch sec; seqNo:3611; 4061509.52 north; 594840.21 east; 36.69 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:59:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869174.30 2024-11-06T04:59:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:36Z- waiting for 1730869178.30, time is 1730869176.33 2024-11-06T04:59:36Z- AHRS_M2 msg: 1730869176.76 epoch sec; seqNo:3392 2024-11-06T04:59:36Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.35 psi 2024-11-06T04:59:36Z- handleDepth()- Depth_Keller msg: 1730869176.77 epoch sec; seqNo:3612; depth 50.29 2024-11-06T04:59:36Z- handleDVL() - DVL msg epochMillisec: 1730869176.79 2024-11-06T04:59:36Z- handleDvl() - RDI_Pathfinder msg: 1730869176.79 sec; seqNo:1273; #beams:4 2024-11-06T04:59:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.29 , 33.46 , 40.19 , 41.12 2024-11-06T04:59:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , 0.02 2024-11-06T04:59:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869176.82 epoch sec; seqNo:3612; 4061509.14 north; 594840.21 east; 36.69 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T04:59:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869174.30 2024-11-06T04:59:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:36Z- waiting for 1730869178.30, time is 1730869176.82 2024-11-06T04:59:37Z- handleDepth()- Depth_Keller msg: 1730869177.13 epoch sec; seqNo:3613; depth 50.30 2024-11-06T04:59:37Z- handleDVL() - DVL msg epochMillisec: 1730869177.14 2024-11-06T04:59:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:59:37Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:59:37Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:59:37Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:59:37Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:59:37Z- handleDvl() - RDI_Pathfinder msg: 1730869177.14 sec; seqNo:1274; #beams:0 2024-11-06T04:59:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:59:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , 0.02 2024-11-06T04:59:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869177.16 epoch sec; seqNo:3613; 4061508.76 north; 594840.22 east; 36.69 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:59:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869174.30 2024-11-06T04:59:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:37Z- waiting for 1730869178.30, time is 1730869177.16 2024-11-06T04:59:37Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T04:59:37Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T04:59:37Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T04:59:37Z- AHRS_M2 msg: 1730869177.51 epoch sec; seqNo:3393 2024-11-06T04:59:37Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.35 psi 2024-11-06T04:59:37Z- handleDepth()- Depth_Keller msg: 1730869177.53 epoch sec; seqNo:3614; depth 50.30 2024-11-06T04:59:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869177.54 epoch sec; seqNo:3614; 4061508.38 north; 594840.22 east; 36.69 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T04:59:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869174.30 2024-11-06T04:59:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:37Z- waiting for 1730869178.30, time is 1730869177.54 2024-11-06T04:59:37Z- AHRS_M2 msg: 1730869177.92 epoch sec; seqNo:3394 2024-11-06T04:59:37Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.35 psi 2024-11-06T04:59:37Z- handleDepth()- Depth_Keller msg: 1730869177.94 epoch sec; seqNo:3615; depth 50.31 2024-11-06T04:59:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869177.96 epoch sec; seqNo:3615; 4061507.99 north; 594839.01 east; 36.69 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T04:59:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869174.30 2024-11-06T04:59:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:38Z- waiting for 1730869178.30, time is 1730869177.96 2024-11-06T04:59:38Z- AHRS_M2 msg: 1730869178.33 epoch sec; seqNo:3395 2024-11-06T04:59:38Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.36 psi 2024-11-06T04:59:38Z- handleDepth()- Depth_Keller msg: 1730869178.33 epoch sec; seqNo:3616; depth 50.32 2024-11-06T04:59:38Z- handleDVL() - DVL msg epochMillisec: 1730869178.34 2024-11-06T04:59:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:59:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:59:38Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:59:38Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:59:38Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:59:38Z- handleDvl() - RDI_Pathfinder msg: 1730869178.34 sec; seqNo:1275; #beams:0 2024-11-06T04:59:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:59:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.02 , 0.02 2024-11-06T04:59:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869178.35 epoch sec; seqNo:3616; 4061507.61 north; 594839.01 east; 36.69 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:59:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869174.30 2024-11-06T04:59:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:38Z- waiting for 1730869178.30, time is 1730869178.35 2024-11-06T04:59:38Z- LcmTrn::updateTrn() >>>> pose time 1730869178.35, meas time 1730869178.34 <<<< 2024-11-06T04:59:38Z- LcmTrn::updateTrn() equating pose 1730869178.34 and meas times 1730869178.34 2024-11-06T04:59:38Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869178.34 2024-11-06T04:59:38Z- TerrainNav::motionUpdate() - currEstimate = (4061508, 594839.01, 50.32) 2024-11-06T04:59:38Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:59:38Z- TerrainNav:: SubcloudNIS is 0.93, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:59:38Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:59:38Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:59:38Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:59:38Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:59:38Z- variances([0], [2], and psi_berg) = 0.865519 0.345959 0.000000 2024-11-06T04:59:38Z- publishEstimatesDecoder() - at 1730869178.34 sec 2024-11-06T04:59:38Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:59:38Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061453.32 594843.48 61.89 2024-11-06T04:59:38Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061450.83 594843.43 60.59 lat/lon: 36.69 -121.94 2024-11-06T04:59:38Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.87 0.35 0.92 0.00 2024-11-06T04:59:38Z- AHRS_M2 msg: 1730869178.76 epoch sec; seqNo:3396 2024-11-06T04:59:38Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.36 psi 2024-11-06T04:59:38Z- handleDepth()- Depth_Keller msg: 1730869178.77 epoch sec; seqNo:3617; depth 50.32 2024-11-06T04:59:38Z- handleDVL() - DVL msg epochMillisec: 1730869178.79 2024-11-06T04:59:38Z- handleDvl() - RDI_Pathfinder msg: 1730869178.79 sec; seqNo:1276; #beams:4 2024-11-06T04:59:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.04 , 33.08 , 40.37 , 41.31 2024-11-06T04:59:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.00 , -0.05 2024-11-06T04:59:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869178.81 epoch sec; seqNo:3617; 4061507.23 north; 594839.02 east; 36.69 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:59:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869178.34 2024-11-06T04:59:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:38Z- waiting for 1730869182.34, time is 1730869178.81 2024-11-06T04:59:39Z- AHRS_M2 msg: 1730869179.14 epoch sec; seqNo:3397 2024-11-06T04:59:39Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.37 psi 2024-11-06T04:59:39Z- handleDepth()- Depth_Keller msg: 1730869179.14 epoch sec; seqNo:3618; depth 50.32 2024-11-06T04:59:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869179.16 epoch sec; seqNo:3618; 4061506.85 north; 594839.02 east; 36.69 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:59:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869178.34 2024-11-06T04:59:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:39Z- waiting for 1730869182.34, time is 1730869179.16 2024-11-06T04:59:39Z- AHRS_M2 msg: 1730869179.58 epoch sec; seqNo:3398 2024-11-06T04:59:39Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.37 psi 2024-11-06T04:59:39Z- handleDepth()- Depth_Keller msg: 1730869179.59 epoch sec; seqNo:3619; depth 50.34 2024-11-06T04:59:39Z- handleDVL() - DVL msg epochMillisec: 1730869179.63 2024-11-06T04:59:39Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:39Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:39Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:39Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:59:39Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:59:39Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:59:39Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:59:39Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:59:39Z- handleDvl() - RDI_Pathfinder msg: 1730869179.63 sec; seqNo:1277; #beams:0 2024-11-06T04:59:39Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:59:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.00 , -0.05 2024-11-06T04:59:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869179.66 epoch sec; seqNo:3619; 4061506.47 north; 594839.02 east; 36.69 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:59:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869178.34 2024-11-06T04:59:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:39Z- waiting for 1730869182.34, time is 1730869179.66 2024-11-06T04:59:39Z- AHRS_M2 msg: 1730869179.95 epoch sec; seqNo:3399 2024-11-06T04:59:39Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 3.37 psi 2024-11-06T04:59:39Z- handleDepth()- Depth_Keller msg: 1730869179.96 epoch sec; seqNo:3620; depth 50.33 2024-11-06T04:59:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869179.97 epoch sec; seqNo:3620; 4061506.09 north; 594839.03 east; 36.69 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T04:59:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869178.34 2024-11-06T04:59:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:40Z- waiting for 1730869182.34, time is 1730869179.97 2024-11-06T04:59:40Z- AHRS_M2 msg: 1730869180.35 epoch sec; seqNo:3400 2024-11-06T04:59:40Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 3.37 psi 2024-11-06T04:59:40Z- handleDepth()- Depth_Keller msg: 1730869180.36 epoch sec; seqNo:3621; depth 50.34 2024-11-06T04:59:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869180.38 epoch sec; seqNo:3621; 4061505.71 north; 594839.03 east; 36.69 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:59:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869178.34 2024-11-06T04:59:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:40Z- waiting for 1730869182.34, time is 1730869180.38 2024-11-06T04:59:40Z- AHRS_M2 msg: 1730869180.76 epoch sec; seqNo:3401 2024-11-06T04:59:40Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 3.37 psi 2024-11-06T04:59:40Z- handleDepth()- Depth_Keller msg: 1730869180.76 epoch sec; seqNo:3622; depth 50.34 2024-11-06T04:59:40Z- handleDVL() - DVL msg epochMillisec: 1730869180.78 2024-11-06T04:59:40Z- handleDvl() - RDI_Pathfinder msg: 1730869180.78 sec; seqNo:1278; #beams:4 2024-11-06T04:59:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.28 , 32.71 , 40.18 , 40.56 2024-11-06T04:59:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.10 , -0.05 2024-11-06T04:59:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869180.81 epoch sec; seqNo:3622; 4061505.71 north; 594839.03 east; 36.69 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:59:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869178.34 2024-11-06T04:59:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:40Z- waiting for 1730869182.34, time is 1730869180.81 2024-11-06T04:59:41Z- AHRS_M2 msg: 1730869181.16 epoch sec; seqNo:3402 2024-11-06T04:59:41Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 3.38 psi 2024-11-06T04:59:41Z- handleDepth()- Depth_Keller msg: 1730869181.16 epoch sec; seqNo:3623; depth 50.36 2024-11-06T04:59:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869181.18 epoch sec; seqNo:3623; 4061505.33 north; 594839.04 east; 36.69 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:59:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869178.34 2024-11-06T04:59:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:41Z- waiting for 1730869182.34, time is 1730869181.18 2024-11-06T04:59:41Z- AHRS_M2 msg: 1730869181.56 epoch sec; seqNo:3403 2024-11-06T04:59:41Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 3.38 psi 2024-11-06T04:59:41Z- handleDepth()- Depth_Keller msg: 1730869181.57 epoch sec; seqNo:3624; depth 50.36 2024-11-06T04:59:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869181.58 epoch sec; seqNo:3624; 4061504.96 north; 594839.04 east; 36.69 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:59:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869178.34 2024-11-06T04:59:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:41Z- waiting for 1730869182.34, time is 1730869181.58 2024-11-06T04:59:41Z- AHRS_M2 msg: 1730869181.98 epoch sec; seqNo:3404 2024-11-06T04:59:41Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 3.38 psi 2024-11-06T04:59:41Z- handleDepth()- Depth_Keller msg: 1730869181.98 epoch sec; seqNo:3625; depth 50.38 2024-11-06T04:59:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869181.99 epoch sec; seqNo:3625; 4061504.58 north; 594839.04 east; 36.69 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:59:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869178.34 2024-11-06T04:59:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:42Z- waiting for 1730869182.34, time is 1730869181.99 2024-11-06T04:59:42Z- AHRS_M2 msg: 1730869182.37 epoch sec; seqNo:3405 2024-11-06T04:59:42Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 3.38 psi 2024-11-06T04:59:42Z- handleDepth()- Depth_Keller msg: 1730869182.37 epoch sec; seqNo:3626; depth 50.38 2024-11-06T04:59:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869182.39 epoch sec; seqNo:3626; 4061504.20 north; 594839.05 east; 36.69 lat; -121.94 lon; 50.38 depth; utmZone: 10 2024-11-06T04:59:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869178.34 2024-11-06T04:59:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:42Z- waiting for 1730869182.34, time is 1730869182.39 2024-11-06T04:59:42Z- LcmTrn::updateTrn() >>>> pose time 1730869182.39, meas time 1730869180.78 <<<< 2024-11-06T04:59:42Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869182.39 2024-11-06T04:59:42Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869180.78 sec, ping # 0; waiting for INS data more recent than 1730869178.34... 2024-11-06T04:59:42Z- TerrainNav::motionUpdate() - currEstimate = (4061504, 594839.05, 50.38) 2024-11-06T04:59:42Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:59:42Z- TerrainNav:: SubcloudNIS is 0.93, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:59:42Z- TNavPF::Projecting Measurements 2024-11-06T04:59:42Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:59:42Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:59:42Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:59:42Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:59:42Z- TNavFilter::4 good beams. 2024-11-06T04:59:42Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:59:42Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:59:42Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:59:42Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:59:42Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:59:42Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:59:42Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:59:42Z- TerrainMapOctree: (vehN, vehE ) = (4061505.55, 594839.03). 2024-11-06T04:59:42Z- TerrainMapOctree: Min Distance = 260.16. 2024-11-06T04:59:42Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:59:42Z- Forcing Subcloud Comparison 2024-11-06T04:59:42Z- Weighting particles with subcloud comparison 2024-11-06T04:59:42Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:42Z- meanExpectedMeasDiff: 1.621588 delta_rms_squared: 1.974648 alpha: 0.989973 2024-11-06T04:59:42Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:42Z- meanExpectedMeasDiff: -2.517640 delta_rms_squared: 1.318231 alpha: 0.985055 2024-11-06T04:59:42Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:42Z- meanExpectedMeasDiff: 0.959305 delta_rms_squared: 1.541622 alpha: 0.987193 2024-11-06T04:59:42Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:42Z- meanExpectedMeasDiff: -0.231543 delta_rms_squared: 2.055327 alpha: 0.990363 2024-11-06T04:59:42Z- Subcloud NIS: 0.901929 2024-11-06T04:59:42Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:59:42Z- TNavPF:: sumWeights = 0.176322 2024-11-06T04:59:42Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:59:42Z- TNavPF::First Term of Map Covariance Matrix is 0.50 2024-11-06T04:59:42Z- TNavPF::Calculated NIS Value : 0.09 numBeams normalized NIS: 0.02 2024-11-06T04:59:42Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869180.78 sec. 2024-11-06T04:59:42Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:59:42Z- variances([0], [2], and psi_berg) = 0.872193 0.341550 0.000000 2024-11-06T04:59:42Z- publishEstimatesDecoder() - at 1730869182.39 sec 2024-11-06T04:59:42Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:59:42Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061448.09 594843.63 61.43 2024-11-06T04:59:42Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061447.43 594843.44 60.75 lat/lon: 36.69 -121.94 2024-11-06T04:59:42Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.87 0.34 0.72 0.00 2024-11-06T04:59:42Z- AHRS_M2 msg: 1730869182.79 epoch sec; seqNo:3406 2024-11-06T04:59:42Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 3.38 psi 2024-11-06T04:59:42Z- handleDepth()- Depth_Keller msg: 1730869182.81 epoch sec; seqNo:3627; depth 50.39 2024-11-06T04:59:42Z- handleDVL() - DVL msg epochMillisec: 1730869182.85 2024-11-06T04:59:42Z- handleDvl() - RDI_Pathfinder msg: 1730869182.85 sec; seqNo:1279; #beams:4 2024-11-06T04:59:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.47 , 32.15 , 40.56 , 40.18 2024-11-06T04:59:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.15 , 0.01 2024-11-06T04:59:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869182.88 epoch sec; seqNo:3627; 4061503.81 north; 594837.83 east; 36.69 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:59:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869182.39 2024-11-06T04:59:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:42Z- waiting for 1730869186.39, time is 1730869182.88 2024-11-06T04:59:43Z- AHRS_M2 msg: 1730869183.17 epoch sec; seqNo:3407 2024-11-06T04:59:43Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 3.38 psi 2024-11-06T04:59:43Z- handleDepth()- Depth_Keller msg: 1730869183.19 epoch sec; seqNo:3628; depth 50.40 2024-11-06T04:59:43Z- handleDVL() - DVL msg epochMillisec: 1730869183.19 2024-11-06T04:59:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:59:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:59:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:59:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:59:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:59:43Z- handleDvl() - RDI_Pathfinder msg: 1730869183.19 sec; seqNo:1280; #beams:0 2024-11-06T04:59:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:59:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.15 , 0.01 2024-11-06T04:59:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869183.20 epoch sec; seqNo:3628; 4061503.43 north; 594837.84 east; 36.69 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T04:59:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869182.39 2024-11-06T04:59:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:43Z- waiting for 1730869186.39, time is 1730869183.20 2024-11-06T04:59:43Z- AHRS_M2 msg: 1730869183.58 epoch sec; seqNo:3408 2024-11-06T04:59:43Z- AHRS_M2 msg: 0.18 phi; -0.02 theta; 3.38 psi 2024-11-06T04:59:43Z- handleDepth()- Depth_Keller msg: 1730869183.59 epoch sec; seqNo:3629; depth 50.40 2024-11-06T04:59:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869183.61 epoch sec; seqNo:3629; 4061503.05 north; 594837.84 east; 36.69 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T04:59:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869182.39 2024-11-06T04:59:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:43Z- waiting for 1730869186.39, time is 1730869183.61 2024-11-06T04:59:43Z- AHRS_M2 msg: 1730869183.99 epoch sec; seqNo:3409 2024-11-06T04:59:44Z- AHRS_M2 msg: 0.18 phi; -0.02 theta; 3.38 psi 2024-11-06T04:59:44Z- handleDepth()- Depth_Keller msg: 1730869183.99 epoch sec; seqNo:3630; depth 50.42 2024-11-06T04:59:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869184.01 epoch sec; seqNo:3630; 4061502.67 north; 594837.85 east; 36.69 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T04:59:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869182.39 2024-11-06T04:59:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:44Z- waiting for 1730869186.39, time is 1730869184.01 2024-11-06T04:59:44Z- AHRS_M2 msg: 1730869184.39 epoch sec; seqNo:3410 2024-11-06T04:59:44Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 3.39 psi 2024-11-06T04:59:44Z- handleDepth()- Depth_Keller msg: 1730869184.40 epoch sec; seqNo:3631; depth 50.43 2024-11-06T04:59:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869184.41 epoch sec; seqNo:3631; 4061502.67 north; 594837.85 east; 36.69 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:59:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869182.39 2024-11-06T04:59:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:44Z- waiting for 1730869186.39, time is 1730869184.41 2024-11-06T04:59:44Z- AHRS_M2 msg: 1730869184.79 epoch sec; seqNo:3411 2024-11-06T04:59:44Z- AHRS_M2 msg: 0.17 phi; -0.01 theta; 3.39 psi 2024-11-06T04:59:44Z- handleDepth()- Depth_Keller msg: 1730869184.80 epoch sec; seqNo:3632; depth 50.45 2024-11-06T04:59:44Z- handleDVL() - DVL msg epochMillisec: 1730869184.83 2024-11-06T04:59:44Z- handleDvl() - RDI_Pathfinder msg: 1730869184.83 sec; seqNo:1281; #beams:4 2024-11-06T04:59:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.29 , 31.96 , 40.75 , 40.00 2024-11-06T04:59:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , 0.10 , 0.08 2024-11-06T04:59:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869184.85 epoch sec; seqNo:3632; 4061502.29 north; 594837.85 east; 36.69 lat; -121.94 lon; 50.45 depth; utmZone: 10 2024-11-06T04:59:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869182.39 2024-11-06T04:59:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:44Z- waiting for 1730869186.39, time is 1730869184.85 2024-11-06T04:59:45Z- AHRS_M2 msg: 1730869185.19 epoch sec; seqNo:3412 2024-11-06T04:59:45Z- AHRS_M2 msg: 0.17 phi; -0.01 theta; 3.39 psi 2024-11-06T04:59:45Z- handleDepth()- Depth_Keller msg: 1730869185.20 epoch sec; seqNo:3633; depth 50.46 2024-11-06T04:59:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869185.21 epoch sec; seqNo:3633; 4061501.91 north; 594837.85 east; 36.69 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T04:59:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869182.39 2024-11-06T04:59:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:45Z- waiting for 1730869186.39, time is 1730869185.21 2024-11-06T04:59:45Z- AHRS_M2 msg: 1730869185.60 epoch sec; seqNo:3413 2024-11-06T04:59:45Z- AHRS_M2 msg: 0.17 phi; -0.01 theta; 3.39 psi 2024-11-06T04:59:45Z- handleDepth()- Depth_Keller msg: 1730869185.61 epoch sec; seqNo:3634; depth 50.48 2024-11-06T04:59:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869185.62 epoch sec; seqNo:3634; 4061501.53 north; 594837.86 east; 36.69 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T04:59:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869182.39 2024-11-06T04:59:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:45Z- waiting for 1730869186.39, time is 1730869185.62 2024-11-06T04:59:46Z- AHRS_M2 msg: 1730869186.00 epoch sec; seqNo:3414 2024-11-06T04:59:46Z- AHRS_M2 msg: 0.17 phi; -0.01 theta; 3.39 psi 2024-11-06T04:59:46Z- handleDepth()- Depth_Keller msg: 1730869186.02 epoch sec; seqNo:3635; depth 50.50 2024-11-06T04:59:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869186.03 epoch sec; seqNo:3635; 4061501.15 north; 594837.86 east; 36.69 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T04:59:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869182.39 2024-11-06T04:59:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:46Z- waiting for 1730869186.39, time is 1730869186.03 2024-11-06T04:59:46Z- AHRS_M2 msg: 1730869186.41 epoch sec; seqNo:3415 2024-11-06T04:59:46Z- AHRS_M2 msg: 0.17 phi; -0.01 theta; 3.40 psi 2024-11-06T04:59:46Z- handleDepth()- Depth_Keller msg: 1730869186.42 epoch sec; seqNo:3636; depth 50.50 2024-11-06T04:59:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869186.44 epoch sec; seqNo:3636; 4061500.76 north; 594836.65 east; 36.69 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T04:59:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869182.39 2024-11-06T04:59:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:46Z- waiting for 1730869186.39, time is 1730869186.44 2024-11-06T04:59:46Z- LcmTrn::updateTrn() >>>> pose time 1730869186.44, meas time 1730869184.83 <<<< 2024-11-06T04:59:46Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869186.44 2024-11-06T04:59:46Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869184.83 sec, ping # 0; waiting for INS data more recent than 1730869182.39... 2024-11-06T04:59:46Z- TerrainNav::motionUpdate() - currEstimate = (4061501, 594836.65, 50.50) 2024-11-06T04:59:46Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:59:46Z- TerrainNav:: SubcloudNIS is 0.90, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:59:46Z- TNavPF::Projecting Measurements 2024-11-06T04:59:46Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:59:46Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:59:46Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:59:46Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:59:46Z- TNavFilter::4 good beams. 2024-11-06T04:59:46Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:59:46Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:59:46Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:59:46Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:59:46Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:59:46Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:59:46Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:59:46Z- TerrainMapOctree: (vehN, vehE ) = (4061502.13, 594837.60). 2024-11-06T04:59:46Z- TerrainMapOctree: Min Distance = 262.90. 2024-11-06T04:59:46Z- TerrainMapOctree: Using tile 5. 2024-11-06T04:59:46Z- Forcing Subcloud Comparison 2024-11-06T04:59:46Z- Weighting particles with subcloud comparison 2024-11-06T04:59:46Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:46Z- meanExpectedMeasDiff: 1.907984 delta_rms_squared: 1.645322 alpha: 0.987990 2024-11-06T04:59:46Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:46Z- meanExpectedMeasDiff: -3.058254 delta_rms_squared: 1.099352 alpha: 0.982133 2024-11-06T04:59:46Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:46Z- meanExpectedMeasDiff: 1.401042 delta_rms_squared: 1.308846 alpha: 0.984949 2024-11-06T04:59:46Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:46Z- meanExpectedMeasDiff: -0.028964 delta_rms_squared: 1.355498 alpha: 0.985460 2024-11-06T04:59:46Z- Subcloud NIS: 0.877800 2024-11-06T04:59:46Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:59:46Z- TNavPF:: sumWeights = 0.098336 2024-11-06T04:59:46Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:59:46Z- TNavPF::First Term of Map Covariance Matrix is 0.39 2024-11-06T04:59:46Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T04:59:46Z- TerrainNav::Resampling particle filter... 2024-11-06T04:59:46Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869184.83 sec. 2024-11-06T04:59:46Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:59:46Z- variances([0], [2], and psi_berg) = 0.883873 0.353947 0.000000 2024-11-06T04:59:46Z- publishEstimatesDecoder() - at 1730869186.44 sec 2024-11-06T04:59:46Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:59:46Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061446.29 594841.20 61.01 2024-11-06T04:59:46Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061444.00 594841.04 60.91 lat/lon: 36.69 -121.94 2024-11-06T04:59:46Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.88 0.35 0.67 0.00 2024-11-06T04:59:46Z- AHRS_M2 msg: 1730869186.83 epoch sec; seqNo:3416 2024-11-06T04:59:46Z- AHRS_M2 msg: 0.17 phi; -0.01 theta; 3.40 psi 2024-11-06T04:59:46Z- handleDepth()- Depth_Keller msg: 1730869186.84 epoch sec; seqNo:3637; depth 50.51 2024-11-06T04:59:46Z- handleDVL() - DVL msg epochMillisec: 1730869186.89 2024-11-06T04:59:46Z- handleDvl() - RDI_Pathfinder msg: 1730869186.89 sec; seqNo:1282; #beams:4 2024-11-06T04:59:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 32.33 , 40.93 , 40.18 2024-11-06T04:59:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.00 , 0.08 2024-11-06T04:59:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869186.91 epoch sec; seqNo:3637; 4061500.38 north; 594836.65 east; 36.69 lat; -121.94 lon; 50.51 depth; utmZone: 10 2024-11-06T04:59:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869186.44 2024-11-06T04:59:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:46Z- waiting for 1730869190.44, time is 1730869186.91 2024-11-06T04:59:47Z- AHRS_M2 msg: 1730869187.22 epoch sec; seqNo:3417 2024-11-06T04:59:47Z- AHRS_M2 msg: 0.17 phi; -0.01 theta; 3.40 psi 2024-11-06T04:59:47Z- handleDepth()- Depth_Keller msg: 1730869187.22 epoch sec; seqNo:3638; depth 50.53 2024-11-06T04:59:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869187.23 epoch sec; seqNo:3638; 4061500.00 north; 594836.65 east; 36.69 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T04:59:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869186.44 2024-11-06T04:59:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:47Z- waiting for 1730869190.44, time is 1730869187.23 2024-11-06T04:59:47Z- AHRS_M2 msg: 1730869187.62 epoch sec; seqNo:3418 2024-11-06T04:59:47Z- AHRS_M2 msg: 0.17 phi; -0.01 theta; 3.41 psi 2024-11-06T04:59:47Z- handleDepth()- Depth_Keller msg: 1730869187.63 epoch sec; seqNo:3639; depth 50.53 2024-11-06T04:59:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869187.64 epoch sec; seqNo:3639; 4061499.62 north; 594836.66 east; 36.69 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T04:59:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869186.44 2024-11-06T04:59:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:47Z- waiting for 1730869190.44, time is 1730869187.64 2024-11-06T04:59:48Z- AHRS_M2 msg: 1730869188.02 epoch sec; seqNo:3419 2024-11-06T04:59:48Z- AHRS_M2 msg: 0.17 phi; -0.00 theta; 3.41 psi 2024-11-06T04:59:48Z- handleDepth()- Depth_Keller msg: 1730869188.03 epoch sec; seqNo:3640; depth 50.54 2024-11-06T04:59:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869188.06 epoch sec; seqNo:3640; 4061499.25 north; 594836.66 east; 36.69 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T04:59:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869186.44 2024-11-06T04:59:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:48Z- waiting for 1730869190.44, time is 1730869188.06 2024-11-06T04:59:48Z- handleDepth()- Depth_Keller msg: 1730869188.44 epoch sec; seqNo:3641; depth 50.53 2024-11-06T04:59:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869188.45 epoch sec; seqNo:3641; 4061498.87 north; 594836.67 east; 36.69 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T04:59:48Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T04:59:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869186.44 2024-11-06T04:59:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:48Z- waiting for 1730869190.44, time is 1730869188.45 2024-11-06T04:59:48Z- AHRS_M2 msg: 1730869188.84 epoch sec; seqNo:3420 2024-11-06T04:59:48Z- AHRS_M2 msg: 0.17 phi; -0.00 theta; 3.42 psi 2024-11-06T04:59:48Z- handleDepth()- Depth_Keller msg: 1730869188.85 epoch sec; seqNo:3642; depth 50.54 2024-11-06T04:59:48Z- handleDVL() - DVL msg epochMillisec: 1730869188.87 2024-11-06T04:59:48Z- handleDvl() - RDI_Pathfinder msg: 1730869188.87 sec; seqNo:1283; #beams:4 2024-11-06T04:59:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.73 , 31.96 , 41.12 , 40.37 2024-11-06T04:59:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , 0.02 2024-11-06T04:59:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869188.89 epoch sec; seqNo:3642; 4061498.49 north; 594836.67 east; 36.69 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T04:59:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869186.44 2024-11-06T04:59:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:49Z- waiting for 1730869190.44, time is 1730869188.89 2024-11-06T04:59:49Z- AHRS_M2 msg: 1730869189.23 epoch sec; seqNo:3421 2024-11-06T04:59:49Z- AHRS_M2 msg: 0.17 phi; -0.00 theta; 3.42 psi 2024-11-06T04:59:49Z- handleDepth()- Depth_Keller msg: 1730869189.25 epoch sec; seqNo:3643; depth 50.55 2024-11-06T04:59:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869189.26 epoch sec; seqNo:3643; 4061498.49 north; 594836.67 east; 36.69 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T04:59:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869186.44 2024-11-06T04:59:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:49Z- waiting for 1730869190.44, time is 1730869189.26 2024-11-06T04:59:49Z- AHRS_M2 msg: 1730869189.64 epoch sec; seqNo:3422 2024-11-06T04:59:49Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 3.43 psi 2024-11-06T04:59:49Z- handleDepth()- Depth_Keller msg: 1730869189.65 epoch sec; seqNo:3644; depth 50.54 2024-11-06T04:59:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869189.67 epoch sec; seqNo:3644; 4061498.11 north; 594836.68 east; 36.69 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T04:59:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869186.44 2024-11-06T04:59:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:49Z- waiting for 1730869190.44, time is 1730869189.67 2024-11-06T04:59:50Z- AHRS_M2 msg: 1730869190.04 epoch sec; seqNo:3423 2024-11-06T04:59:50Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 3.43 psi 2024-11-06T04:59:50Z- handleDepth()- Depth_Keller msg: 1730869190.05 epoch sec; seqNo:3645; depth 50.54 2024-11-06T04:59:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869190.06 epoch sec; seqNo:3645; 4061497.73 north; 594836.68 east; 36.69 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T04:59:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869186.44 2024-11-06T04:59:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:50Z- waiting for 1730869190.44, time is 1730869190.06 2024-11-06T04:59:50Z- AHRS_M2 msg: 1730869190.45 epoch sec; seqNo:3424 2024-11-06T04:59:50Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 3.43 psi 2024-11-06T04:59:50Z- handleDepth()- Depth_Keller msg: 1730869190.46 epoch sec; seqNo:3646; depth 50.53 2024-11-06T04:59:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869190.47 epoch sec; seqNo:3646; 4061497.35 north; 594836.68 east; 36.69 lat; -121.94 lon; 50.53 depth; utmZone: 10 2024-11-06T04:59:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869186.44 2024-11-06T04:59:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:50Z- waiting for 1730869190.44, time is 1730869190.47 2024-11-06T04:59:50Z- LcmTrn::updateTrn() >>>> pose time 1730869190.47, meas time 1730869188.87 <<<< 2024-11-06T04:59:50Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869190.47 2024-11-06T04:59:50Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869188.87 sec, ping # 0; waiting for INS data more recent than 1730869186.44... 2024-11-06T04:59:50Z- TerrainNav::motionUpdate() - currEstimate = (4061497, 594836.68, 50.53) 2024-11-06T04:59:50Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:59:50Z- TerrainNav:: SubcloudNIS is 0.88, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:59:50Z- TNavPF::Projecting Measurements 2024-11-06T04:59:50Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:59:50Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:59:50Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:59:50Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:59:50Z- TNavFilter::4 good beams. 2024-11-06T04:59:50Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:59:50Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:59:50Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:59:50Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:59:50Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:59:50Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:59:50Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:59:50Z- TerrainMapOctree: (vehN, vehE ) = (4061498.71, 594836.67). 2024-11-06T04:59:50Z- TerrainMapOctree: Min Distance = 263.37. 2024-11-06T04:59:50Z- TerrainMapOctree: Using tile 2. 2024-11-06T04:59:50Z- TerrainMapOctree: Switching to tile 2. 2024-11-06T04:59:51Z- TerrainMapOctree::Octree tile load /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile2.bo took 0.950693 seconds. 2024-11-06T04:59:51Z- Forcing Subcloud Comparison 2024-11-06T04:59:51Z- Weighting particles with subcloud comparison 2024-11-06T04:59:51Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:52Z- meanExpectedMeasDiff: 1.593706 delta_rms_squared: 1.535580 alpha: 0.987143 2024-11-06T04:59:52Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:52Z- meanExpectedMeasDiff: -3.053631 delta_rms_squared: 1.037088 alpha: 0.981080 2024-11-06T04:59:52Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:52Z- meanExpectedMeasDiff: 1.575886 delta_rms_squared: 1.247664 alpha: 0.984223 2024-11-06T04:59:52Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:52Z- meanExpectedMeasDiff: 0.750123 delta_rms_squared: 1.252679 alpha: 0.984285 2024-11-06T04:59:52Z- Subcloud NIS: 0.862784 2024-11-06T04:59:52Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:59:52Z- TNavPF:: sumWeights = 0.098166 2024-11-06T04:59:52Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:59:52Z- TNavPF::First Term of Map Covariance Matrix is 0.26 2024-11-06T04:59:52Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T04:59:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869188.87 sec. 2024-11-06T04:59:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:59:52Z- variances([0], [2], and psi_berg) = 0.887874 0.357382 0.000000 2024-11-06T04:59:52Z- publishEstimatesDecoder() - at 1730869190.47 sec 2024-11-06T04:59:52Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:59:52Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061441.84 594841.12 61.20 2024-11-06T04:59:52Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061440.59 594841.07 60.92 lat/lon: 36.69 -121.94 2024-11-06T04:59:52Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.89 0.36 0.67 0.00 2024-11-06T04:59:52Z- AHRS_M2 msg: 1730869190.85 epoch sec; seqNo:3425 2024-11-06T04:59:52Z- AHRS_M2 msg: 0.16 phi; 0.00 theta; 3.44 psi 2024-11-06T04:59:52Z- handleDepth()- Depth_Keller msg: 1730869190.86 epoch sec; seqNo:3647; depth 50.50 2024-11-06T04:59:52Z- handleDVL() - DVL msg epochMillisec: 1730869190.88 2024-11-06T04:59:52Z- handleDvl() - RDI_Pathfinder msg: 1730869190.88 sec; seqNo:1284; #beams:4 2024-11-06T04:59:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.28 , 32.89 , 41.49 , 40.37 2024-11-06T04:59:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.04 , -0.06 2024-11-06T04:59:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869190.89 epoch sec; seqNo:3647; 4061496.97 north; 594836.69 east; 36.69 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T04:59:52Z- AHRS_M2 msg: 1730869191.27 epoch sec; seqNo:3426 2024-11-06T04:59:52Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 3.44 psi 2024-11-06T04:59:52Z- handleDepth()- Depth_Keller msg: 1730869191.30 epoch sec; seqNo:3648; depth 50.50 2024-11-06T04:59:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869191.32 epoch sec; seqNo:3648; 4061496.59 north; 594836.69 east; 36.69 lat; -121.94 lon; 50.50 depth; utmZone: 10 2024-11-06T04:59:52Z- handleDepth()- Depth_Keller msg: 1730869191.72 epoch sec; seqNo:3649; depth 50.48 2024-11-06T04:59:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869191.87 epoch sec; seqNo:3649; 4061495.82 north; 594835.48 east; 36.69 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T04:59:52Z- AHRS_M2 msg: 1730869192.09 epoch sec; seqNo:3427 2024-11-06T04:59:52Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 3.44 psi 2024-11-06T04:59:52Z- handleDepth()- Depth_Keller msg: 1730869192.09 epoch sec; seqNo:3650; depth 50.49 2024-11-06T04:59:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869192.12 epoch sec; seqNo:3650; 4061495.82 north; 594835.48 east; 36.69 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T04:59:52Z- 12 msgs in 200 ms and 13 handle calls 2024-11-06T04:59:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869190.47 2024-11-06T04:59:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:52Z- waiting for 1730869194.47, time is 1730869192.12 2024-11-06T04:59:52Z- AHRS_M2 msg: 1730869192.47 epoch sec; seqNo:3428 2024-11-06T04:59:52Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 3.44 psi 2024-11-06T04:59:52Z- handleDepth()- Depth_Keller msg: 1730869192.48 epoch sec; seqNo:3651; depth 50.47 2024-11-06T04:59:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869192.49 epoch sec; seqNo:3651; 4061495.44 north; 594835.48 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T04:59:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869190.47 2024-11-06T04:59:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:52Z- waiting for 1730869194.47, time is 1730869192.49 2024-11-06T04:59:52Z- AHRS_M2 msg: 1730869192.87 epoch sec; seqNo:3429 2024-11-06T04:59:52Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 3.44 psi 2024-11-06T04:59:52Z- handleDepth()- Depth_Keller msg: 1730869192.88 epoch sec; seqNo:3652; depth 50.46 2024-11-06T04:59:52Z- handleDVL() - DVL msg epochMillisec: 1730869192.90 2024-11-06T04:59:52Z- handleDvl() - RDI_Pathfinder msg: 1730869192.90 sec; seqNo:1285; #beams:4 2024-11-06T04:59:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.72 , 33.45 , 41.49 , 40.37 2024-11-06T04:59:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , -0.09 2024-11-06T04:59:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869192.92 epoch sec; seqNo:3652; 4061495.07 north; 594835.49 east; 36.69 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T04:59:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869190.47 2024-11-06T04:59:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:53Z- waiting for 1730869194.47, time is 1730869192.92 2024-11-06T04:59:53Z- AHRS_M2 msg: 1730869193.28 epoch sec; seqNo:3430 2024-11-06T04:59:53Z- AHRS_M2 msg: 0.16 phi; 0.01 theta; 3.44 psi 2024-11-06T04:59:53Z- handleDepth()- Depth_Keller msg: 1730869193.28 epoch sec; seqNo:3653; depth 50.44 2024-11-06T04:59:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869193.30 epoch sec; seqNo:3653; 4061494.69 north; 594835.49 east; 36.69 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T04:59:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869190.47 2024-11-06T04:59:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:53Z- waiting for 1730869194.47, time is 1730869193.30 2024-11-06T04:59:53Z- AHRS_M2 msg: 1730869193.68 epoch sec; seqNo:3431 2024-11-06T04:59:53Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 3.44 psi 2024-11-06T04:59:53Z- handleDepth()- Depth_Keller msg: 1730869193.69 epoch sec; seqNo:3654; depth 50.43 2024-11-06T04:59:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869193.70 epoch sec; seqNo:3654; 4061494.31 north; 594835.50 east; 36.69 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T04:59:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869190.47 2024-11-06T04:59:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:53Z- waiting for 1730869194.47, time is 1730869193.70 2024-11-06T04:59:54Z- AHRS_M2 msg: 1730869194.08 epoch sec; seqNo:3432 2024-11-06T04:59:54Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 3.44 psi 2024-11-06T04:59:54Z- handleDepth()- Depth_Keller msg: 1730869194.10 epoch sec; seqNo:3655; depth 50.41 2024-11-06T04:59:54Z- handleDVL() - DVL msg epochMillisec: 1730869194.11 2024-11-06T04:59:54Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:54Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:54Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:54Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:59:54Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:59:54Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:59:54Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:59:54Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:59:54Z- handleDvl() - RDI_Pathfinder msg: 1730869194.11 sec; seqNo:1286; #beams:0 2024-11-06T04:59:54Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:59:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , -0.09 2024-11-06T04:59:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869194.11 epoch sec; seqNo:3655; 4061493.93 north; 594835.50 east; 36.69 lat; -121.94 lon; 50.41 depth; utmZone: 10 2024-11-06T04:59:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869190.47 2024-11-06T04:59:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:54Z- waiting for 1730869194.47, time is 1730869194.11 2024-11-06T04:59:54Z- AHRS_M2 msg: 1730869194.49 epoch sec; seqNo:3433 2024-11-06T04:59:54Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.44 psi 2024-11-06T04:59:54Z- handleDepth()- Depth_Keller msg: 1730869194.50 epoch sec; seqNo:3656; depth 50.39 2024-11-06T04:59:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869194.51 epoch sec; seqNo:3656; 4061493.55 north; 594835.51 east; 36.69 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:59:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869190.47 2024-11-06T04:59:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:54Z- waiting for 1730869194.47, time is 1730869194.51 2024-11-06T04:59:54Z- LcmTrn::updateTrn() >>>> pose time 1730869194.51, meas time 1730869194.11 <<<< 2024-11-06T04:59:54Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869194.51 2024-11-06T04:59:54Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T04:59:54Z- TerrainNav::motionUpdate() - currEstimate = (4061494, 594835.51, 50.39) 2024-11-06T04:59:54Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:59:54Z- TerrainNav:: SubcloudNIS is 0.86, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:59:54Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T04:59:54Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T04:59:54Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:59:54Z- variances([0], [2], and psi_berg) = 0.890236 0.360246 0.000000 2024-11-06T04:59:54Z- publishEstimatesDecoder() - at 1730869194.51 sec 2024-11-06T04:59:54Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:59:54Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061438.01 594839.93 60.79 2024-11-06T04:59:54Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061436.79 594839.89 60.79 lat/lon: 36.69 -121.94 2024-11-06T04:59:54Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.89 0.36 0.92 0.00 2024-11-06T04:59:54Z- AHRS_M2 msg: 1730869194.89 epoch sec; seqNo:3434 2024-11-06T04:59:54Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.43 psi 2024-11-06T04:59:54Z- handleDepth()- Depth_Keller msg: 1730869194.91 epoch sec; seqNo:3657; depth 50.39 2024-11-06T04:59:54Z- handleDVL() - DVL msg epochMillisec: 1730869194.94 2024-11-06T04:59:54Z- handleDvl() - RDI_Pathfinder msg: 1730869194.94 sec; seqNo:1287; #beams:4 2024-11-06T04:59:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.54 , 34.01 , 41.49 , 40.00 2024-11-06T04:59:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.15 , -0.03 2024-11-06T04:59:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869194.95 epoch sec; seqNo:3657; 4061493.17 north; 594835.51 east; 36.69 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T04:59:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869194.51 2024-11-06T04:59:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:55Z- waiting for 1730869198.51, time is 1730869194.95 2024-11-06T04:59:55Z- AHRS_M2 msg: 1730869195.31 epoch sec; seqNo:3435 2024-11-06T04:59:55Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 3.43 psi 2024-11-06T04:59:55Z- handleDepth()- Depth_Keller msg: 1730869195.32 epoch sec; seqNo:3658; depth 50.37 2024-11-06T04:59:55Z- handleDVL() - DVL msg epochMillisec: 1730869195.32 2024-11-06T04:59:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:59:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:59:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:59:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:59:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:59:55Z- handleDvl() - RDI_Pathfinder msg: 1730869195.32 sec; seqNo:1288; #beams:0 2024-11-06T04:59:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:59:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.15 , -0.03 2024-11-06T04:59:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869195.34 epoch sec; seqNo:3658; 4061493.17 north; 594835.51 east; 36.69 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:59:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869194.51 2024-11-06T04:59:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:55Z- waiting for 1730869198.51, time is 1730869195.34 2024-11-06T04:59:55Z- AHRS_M2 msg: 1730869195.71 epoch sec; seqNo:3436 2024-11-06T04:59:55Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 3.43 psi 2024-11-06T04:59:55Z- handleDepth()- Depth_Keller msg: 1730869195.71 epoch sec; seqNo:3659; depth 50.37 2024-11-06T04:59:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869195.72 epoch sec; seqNo:3659; 4061492.78 north; 594834.29 east; 36.69 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T04:59:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869194.51 2024-11-06T04:59:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:55Z- waiting for 1730869198.51, time is 1730869195.72 2024-11-06T04:59:56Z- AHRS_M2 msg: 1730869196.11 epoch sec; seqNo:3437 2024-11-06T04:59:56Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 3.43 psi 2024-11-06T04:59:56Z- handleDepth()- Depth_Keller msg: 1730869196.11 epoch sec; seqNo:3660; depth 50.36 2024-11-06T04:59:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869196.13 epoch sec; seqNo:3660; 4061492.40 north; 594834.30 east; 36.69 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:59:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869194.51 2024-11-06T04:59:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:56Z- waiting for 1730869198.51, time is 1730869196.13 2024-11-06T04:59:56Z- AHRS_M2 msg: 1730869196.51 epoch sec; seqNo:3438 2024-11-06T04:59:56Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 3.43 psi 2024-11-06T04:59:56Z- handleDepth()- Depth_Keller msg: 1730869196.51 epoch sec; seqNo:3661; depth 50.36 2024-11-06T04:59:56Z- handleDVL() - DVL msg epochMillisec: 1730869196.52 2024-11-06T04:59:56Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:56Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:56Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T04:59:56Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T04:59:56Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T04:59:56Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T04:59:56Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T04:59:56Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T04:59:56Z- handleDvl() - RDI_Pathfinder msg: 1730869196.52 sec; seqNo:1289; #beams:0 2024-11-06T04:59:56Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T04:59:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.15 , -0.03 2024-11-06T04:59:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869196.59 epoch sec; seqNo:3661; 4061492.02 north; 594834.30 east; 36.69 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T04:59:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869194.51 2024-11-06T04:59:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:56Z- waiting for 1730869198.51, time is 1730869196.59 2024-11-06T04:59:56Z- AHRS_M2 msg: 1730869196.91 epoch sec; seqNo:3439 2024-11-06T04:59:56Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.42 psi 2024-11-06T04:59:56Z- handleDepth()- Depth_Keller msg: 1730869196.93 epoch sec; seqNo:3662; depth 50.35 2024-11-06T04:59:56Z- handleDVL() - DVL msg epochMillisec: 1730869196.95 2024-11-06T04:59:56Z- handleDvl() - RDI_Pathfinder msg: 1730869196.95 sec; seqNo:1290; #beams:4 2024-11-06T04:59:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.35 , 35.32 , 41.68 , 39.81 2024-11-06T04:59:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.13 , 0.06 2024-11-06T04:59:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869196.96 epoch sec; seqNo:3662; 4061491.64 north; 594834.31 east; 36.69 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T04:59:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869194.51 2024-11-06T04:59:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:57Z- waiting for 1730869198.51, time is 1730869196.96 2024-11-06T04:59:57Z- AHRS_M2 msg: 1730869197.32 epoch sec; seqNo:3440 2024-11-06T04:59:57Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.43 psi 2024-11-06T04:59:57Z- handleDepth()- Depth_Keller msg: 1730869197.33 epoch sec; seqNo:3663; depth 50.34 2024-11-06T04:59:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869197.35 epoch sec; seqNo:3663; 4061491.26 north; 594834.31 east; 36.69 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:59:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869194.51 2024-11-06T04:59:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:57Z- waiting for 1730869198.51, time is 1730869197.35 2024-11-06T04:59:57Z- AHRS_M2 msg: 1730869197.72 epoch sec; seqNo:3441 2024-11-06T04:59:57Z- AHRS_M2 msg: 0.15 phi; 0.02 theta; 3.42 psi 2024-11-06T04:59:57Z- handleDepth()- Depth_Keller msg: 1730869197.73 epoch sec; seqNo:3664; depth 50.34 2024-11-06T04:59:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869197.76 epoch sec; seqNo:3664; 4061490.88 north; 594834.31 east; 36.69 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:59:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869194.51 2024-11-06T04:59:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:57Z- waiting for 1730869198.51, time is 1730869197.76 2024-11-06T04:59:58Z- AHRS_M2 msg: 1730869198.12 epoch sec; seqNo:3442 2024-11-06T04:59:58Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 3.43 psi 2024-11-06T04:59:58Z- handleDepth()- Depth_Keller msg: 1730869198.13 epoch sec; seqNo:3665; depth 50.33 2024-11-06T04:59:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869198.14 epoch sec; seqNo:3665; 4061490.51 north; 594834.32 east; 36.69 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T04:59:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869194.51 2024-11-06T04:59:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:58Z- waiting for 1730869198.51, time is 1730869198.14 2024-11-06T04:59:58Z- AHRS_M2 msg: 1730869198.53 epoch sec; seqNo:3443 2024-11-06T04:59:58Z- AHRS_M2 msg: 0.16 phi; 0.02 theta; 3.43 psi 2024-11-06T04:59:58Z- handleDepth()- Depth_Keller msg: 1730869198.54 epoch sec; seqNo:3666; depth 50.32 2024-11-06T04:59:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869198.55 epoch sec; seqNo:3666; 4061490.13 north; 594834.32 east; 36.69 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T04:59:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869194.51 2024-11-06T04:59:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:58Z- waiting for 1730869198.51, time is 1730869198.55 2024-11-06T04:59:58Z- LcmTrn::updateTrn() >>>> pose time 1730869198.55, meas time 1730869196.95 <<<< 2024-11-06T04:59:58Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869198.55 2024-11-06T04:59:58Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869196.95 sec, ping # 0; waiting for INS data more recent than 1730869194.51... 2024-11-06T04:59:58Z- TerrainNav::motionUpdate() - currEstimate = (4061490, 594834.32, 50.32) 2024-11-06T04:59:58Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T04:59:58Z- TerrainNav:: SubcloudNIS is 0.86, in allowable region under 1.40. Keep on Trucking. 2024-11-06T04:59:58Z- TNavPF::Projecting Measurements 2024-11-06T04:59:58Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T04:59:58Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T04:59:58Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T04:59:58Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T04:59:58Z- TNavFilter::4 good beams. 2024-11-06T04:59:58Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T04:59:58Z- TNavPF:: beamIndex[0] = 0 2024-11-06T04:59:58Z- TNavPF:: beamIndex[1] = 1 2024-11-06T04:59:58Z- TNavPF:: beamIndex[2] = 2 2024-11-06T04:59:58Z- TNavPF:: beamIndex[3] = 3 2024-11-06T04:59:58Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T04:59:58Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T04:59:58Z- TerrainMapOctree: (vehN, vehE ) = (4061491.49, 594834.79). 2024-11-06T04:59:58Z- TerrainMapOctree: Min Distance = 255.94. 2024-11-06T04:59:58Z- TerrainMapOctree: Using tile 2. 2024-11-06T04:59:58Z- Forcing Subcloud Comparison 2024-11-06T04:59:58Z- Weighting particles with subcloud comparison 2024-11-06T04:59:58Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:58Z- meanExpectedMeasDiff: 1.375691 delta_rms_squared: 1.901090 alpha: 0.989589 2024-11-06T04:59:58Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:58Z- meanExpectedMeasDiff: -0.065130 delta_rms_squared: 1.320980 alpha: 0.985086 2024-11-06T04:59:58Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:58Z- meanExpectedMeasDiff: 1.409334 delta_rms_squared: 1.613540 alpha: 0.987757 2024-11-06T04:59:58Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T04:59:58Z- meanExpectedMeasDiff: 0.566594 delta_rms_squared: 1.531841 alpha: 0.987112 2024-11-06T04:59:58Z- Subcloud NIS: 0.833444 2024-11-06T04:59:58Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T04:59:58Z- TNavPF:: sumWeights = 0.492011 2024-11-06T04:59:58Z- TNavPF::Calculating NIS Matrices 2024-11-06T04:59:58Z- TNavPF::First Term of Map Covariance Matrix is 0.47 2024-11-06T04:59:58Z- TNavPF::Calculated NIS Value : 0.14 numBeams normalized NIS: 0.03 2024-11-06T04:59:58Z- TerrainNav::Resampling particle filter... 2024-11-06T04:59:58Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869196.95 sec. 2024-11-06T04:59:58Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T04:59:58Z- variances([0], [2], and psi_berg) = 0.884650 0.359182 0.000000 2024-11-06T04:59:58Z- publishEstimatesDecoder() - at 1730869198.55 sec 2024-11-06T04:59:58Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T04:59:58Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061435.75 594838.84 60.55 2024-11-06T04:59:58Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061433.37 594838.72 60.32 lat/lon: 36.69 -121.94 2024-11-06T04:59:58Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.88 0.36 0.71 0.00 2024-11-06T04:59:58Z- AHRS_M2 msg: 1730869198.93 epoch sec; seqNo:3444 2024-11-06T04:59:58Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 3.43 psi 2024-11-06T04:59:58Z- handleDepth()- Depth_Keller msg: 1730869198.95 epoch sec; seqNo:3667; depth 50.33 2024-11-06T04:59:59Z- handleDVL() - DVL msg epochMillisec: 1730869198.99 2024-11-06T04:59:59Z- handleDvl() - RDI_Pathfinder msg: 1730869198.99 sec; seqNo:1291; #beams:4 2024-11-06T04:59:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.54 , 35.88 , 41.87 , 40.00 2024-11-06T04:59:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , 0.08 2024-11-06T04:59:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869199.00 epoch sec; seqNo:3667; 4061489.73 north; 594833.11 east; 36.69 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T04:59:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T04:59:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869198.55 2024-11-06T04:59:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:59Z- waiting for 1730869202.55, time is 1730869199.00 2024-11-06T04:59:59Z- AHRS_M2 msg: 1730869199.33 epoch sec; seqNo:3445 2024-11-06T04:59:59Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 3.43 psi 2024-11-06T04:59:59Z- handleDepth()- Depth_Keller msg: 1730869199.35 epoch sec; seqNo:3668; depth 50.34 2024-11-06T04:59:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869199.38 epoch sec; seqNo:3668; 4061489.73 north; 594833.11 east; 36.69 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:59:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869198.55 2024-11-06T04:59:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:59Z- waiting for 1730869202.55, time is 1730869199.38 2024-11-06T04:59:59Z- AHRS_M2 msg: 1730869199.75 epoch sec; seqNo:3446 2024-11-06T04:59:59Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 3.43 psi 2024-11-06T04:59:59Z- handleDepth()- Depth_Keller msg: 1730869199.75 epoch sec; seqNo:3669; depth 50.34 2024-11-06T04:59:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869199.76 epoch sec; seqNo:3669; 4061489.36 north; 594833.11 east; 36.69 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T04:59:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T04:59:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869198.55 2024-11-06T04:59:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T04:59:59Z- waiting for 1730869202.55, time is 1730869199.76 2024-11-06T05:00:00Z- AHRS_M2 msg: 1730869200.14 epoch sec; seqNo:3447 2024-11-06T05:00:00Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 3.43 psi 2024-11-06T05:00:00Z- handleDepth()- Depth_Keller msg: 1730869200.15 epoch sec; seqNo:3670; depth 50.34 2024-11-06T05:00:00Z- handleDVL() - DVL msg epochMillisec: 1730869200.16 2024-11-06T05:00:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:00:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:00:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:00:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:00:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:00:00Z- handleDvl() - RDI_Pathfinder msg: 1730869200.16 sec; seqNo:1292; #beams:0 2024-11-06T05:00:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:00:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.05 , 0.08 2024-11-06T05:00:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869200.16 epoch sec; seqNo:3670; 4061488.98 north; 594833.11 east; 36.69 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T05:00:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869198.55 2024-11-06T05:00:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:00Z- waiting for 1730869202.55, time is 1730869200.16 2024-11-06T05:00:00Z- AHRS_M2 msg: 1730869200.55 epoch sec; seqNo:3448 2024-11-06T05:00:00Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 3.43 psi 2024-11-06T05:00:00Z- handleDepth()- Depth_Keller msg: 1730869200.55 epoch sec; seqNo:3671; depth 50.34 2024-11-06T05:00:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869200.56 epoch sec; seqNo:3671; 4061488.60 north; 594833.12 east; 36.69 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T05:00:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869198.55 2024-11-06T05:00:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:00Z- waiting for 1730869202.55, time is 1730869200.56 2024-11-06T05:00:00Z- AHRS_M2 msg: 1730869200.95 epoch sec; seqNo:3449 2024-11-06T05:00:00Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 3.43 psi 2024-11-06T05:00:00Z- handleDepth()- Depth_Keller msg: 1730869200.97 epoch sec; seqNo:3672; depth 50.35 2024-11-06T05:00:00Z- handleDVL() - DVL msg epochMillisec: 1730869200.99 2024-11-06T05:00:00Z- handleDvl() - RDI_Pathfinder msg: 1730869200.99 sec; seqNo:1293; #beams:4 2024-11-06T05:00:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.54 , 36.26 , 42.61 , 39.81 2024-11-06T05:00:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , 0.04 2024-11-06T05:00:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869201.02 epoch sec; seqNo:3672; 4061488.22 north; 594833.12 east; 36.69 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T05:00:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869198.55 2024-11-06T05:00:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:01Z- waiting for 1730869202.55, time is 1730869201.02 2024-11-06T05:00:01Z- AHRS_M2 msg: 1730869201.36 epoch sec; seqNo:3450 2024-11-06T05:00:01Z- AHRS_M2 msg: 0.15 phi; 0.03 theta; 3.43 psi 2024-11-06T05:00:01Z- handleDepth()- Depth_Keller msg: 1730869201.37 epoch sec; seqNo:3673; depth 50.33 2024-11-06T05:00:01Z- handleDVL() - DVL msg epochMillisec: 1730869201.38 2024-11-06T05:00:01Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:01Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:01Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:01Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:00:01Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:00:01Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:00:01Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:00:01Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:00:01Z- handleDvl() - RDI_Pathfinder msg: 1730869201.38 sec; seqNo:1294; #beams:0 2024-11-06T05:00:01Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:00:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , 0.04 2024-11-06T05:00:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869201.40 epoch sec; seqNo:3673; 4061487.84 north; 594833.13 east; 36.69 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T05:00:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869198.55 2024-11-06T05:00:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:01Z- waiting for 1730869202.55, time is 1730869201.40 2024-11-06T05:00:01Z- AHRS_M2 msg: 1730869201.76 epoch sec; seqNo:3451 2024-11-06T05:00:01Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 3.43 psi 2024-11-06T05:00:01Z- handleDepth()- Depth_Keller msg: 1730869201.78 epoch sec; seqNo:3674; depth 50.33 2024-11-06T05:00:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869201.80 epoch sec; seqNo:3674; 4061487.46 north; 594833.13 east; 36.69 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T05:00:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869198.55 2024-11-06T05:00:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:01Z- waiting for 1730869202.55, time is 1730869201.80 2024-11-06T05:00:02Z- AHRS_M2 msg: 1730869202.16 epoch sec; seqNo:3452 2024-11-06T05:00:02Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 3.43 psi 2024-11-06T05:00:02Z- handleDepth()- Depth_Keller msg: 1730869202.17 epoch sec; seqNo:3675; depth 50.31 2024-11-06T05:00:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869202.18 epoch sec; seqNo:3675; 4061487.08 north; 594833.14 east; 36.69 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T05:00:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869198.55 2024-11-06T05:00:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:02Z- waiting for 1730869202.55, time is 1730869202.18 2024-11-06T05:00:02Z- AHRS_M2 msg: 1730869202.57 epoch sec; seqNo:3453 2024-11-06T05:00:02Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 3.43 psi 2024-11-06T05:00:02Z- handleDepth()- Depth_Keller msg: 1730869202.57 epoch sec; seqNo:3676; depth 50.31 2024-11-06T05:00:02Z- handleDVL() - DVL msg epochMillisec: 1730869202.58 2024-11-06T05:00:02Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:02Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:02Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:02Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:00:02Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:00:02Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:00:02Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:00:02Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:00:02Z- handleDvl() - RDI_Pathfinder msg: 1730869202.58 sec; seqNo:1295; #beams:0 2024-11-06T05:00:02Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:00:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.02 , 0.04 2024-11-06T05:00:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869202.59 epoch sec; seqNo:3676; 4061486.70 north; 594833.14 east; 36.69 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T05:00:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869198.55 2024-11-06T05:00:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:02Z- waiting for 1730869202.55, time is 1730869202.59 2024-11-06T05:00:02Z- LcmTrn::updateTrn() >>>> pose time 1730869202.59, meas time 1730869202.58 <<<< 2024-11-06T05:00:02Z- LcmTrn::updateTrn() equating pose 1730869202.58 and meas times 1730869202.58 2024-11-06T05:00:02Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869202.58 2024-11-06T05:00:02Z- TerrainNav::motionUpdate() - currEstimate = (4061487, 594833.14, 50.31) 2024-11-06T05:00:02Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:00:02Z- TerrainNav:: SubcloudNIS is 0.83, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:00:02Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T05:00:02Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T05:00:02Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T05:00:02Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:00:02Z- variances([0], [2], and psi_berg) = 0.887677 0.362161 0.000000 2024-11-06T05:00:02Z- publishEstimatesDecoder() - at 1730869202.58 sec 2024-11-06T05:00:02Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:00:02Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061432.34 594837.69 60.53 2024-11-06T05:00:02Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061429.94 594837.53 60.31 lat/lon: 36.69 -121.94 2024-11-06T05:00:02Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.89 0.36 0.96 0.00 2024-11-06T05:00:02Z- AHRS_M2 msg: 1730869202.97 epoch sec; seqNo:3454 2024-11-06T05:00:02Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.43 psi 2024-11-06T05:00:02Z- handleDepth()- Depth_Keller msg: 1730869202.98 epoch sec; seqNo:3677; depth 50.28 2024-11-06T05:00:03Z- handleDVL() - DVL msg epochMillisec: 1730869203.01 2024-11-06T05:00:03Z- handleDvl() - RDI_Pathfinder msg: 1730869203.01 sec; seqNo:1296; #beams:4 2024-11-06T05:00:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.35 , 36.63 , 42.61 , 40.00 2024-11-06T05:00:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , 0.00 2024-11-06T05:00:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869203.02 epoch sec; seqNo:3677; 4061486.32 north; 594833.14 east; 36.69 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T05:00:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869202.58 2024-11-06T05:00:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:03Z- waiting for 1730869206.58, time is 1730869203.02 2024-11-06T05:00:03Z- AHRS_M2 msg: 1730869203.38 epoch sec; seqNo:3455 2024-11-06T05:00:03Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.43 psi 2024-11-06T05:00:03Z- handleDepth()- Depth_Keller msg: 1730869203.40 epoch sec; seqNo:3678; depth 50.27 2024-11-06T05:00:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869203.43 epoch sec; seqNo:3678; 4061485.93 north; 594831.93 east; 36.69 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T05:00:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869202.58 2024-11-06T05:00:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:03Z- waiting for 1730869206.58, time is 1730869203.43 2024-11-06T05:00:03Z- AHRS_M2 msg: 1730869203.78 epoch sec; seqNo:3456 2024-11-06T05:00:03Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.43 psi 2024-11-06T05:00:03Z- handleDepth()- Depth_Keller msg: 1730869203.79 epoch sec; seqNo:3679; depth 50.25 2024-11-06T05:00:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869203.81 epoch sec; seqNo:3679; 4061485.55 north; 594831.93 east; 36.69 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T05:00:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869202.58 2024-11-06T05:00:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:03Z- waiting for 1730869206.58, time is 1730869203.81 2024-11-06T05:00:04Z- AHRS_M2 msg: 1730869204.18 epoch sec; seqNo:3457 2024-11-06T05:00:04Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.43 psi 2024-11-06T05:00:04Z- handleDepth()- Depth_Keller msg: 1730869204.19 epoch sec; seqNo:3680; depth 50.25 2024-11-06T05:00:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869204.20 epoch sec; seqNo:3680; 4061485.17 north; 594831.94 east; 36.69 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T05:00:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869202.58 2024-11-06T05:00:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:04Z- waiting for 1730869206.58, time is 1730869204.20 2024-11-06T05:00:04Z- AHRS_M2 msg: 1730869204.59 epoch sec; seqNo:3458 2024-11-06T05:00:04Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.43 psi 2024-11-06T05:00:04Z- handleDepth()- Depth_Keller msg: 1730869204.60 epoch sec; seqNo:3681; depth 50.23 2024-11-06T05:00:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869204.61 epoch sec; seqNo:3681; 4061484.80 north; 594831.94 east; 36.69 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T05:00:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869202.58 2024-11-06T05:00:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:04Z- waiting for 1730869206.58, time is 1730869204.61 2024-11-06T05:00:04Z- AHRS_M2 msg: 1730869204.99 epoch sec; seqNo:3459 2024-11-06T05:00:04Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.43 psi 2024-11-06T05:00:05Z- handleDepth()- Depth_Keller msg: 1730869205.00 epoch sec; seqNo:3682; depth 50.17 2024-11-06T05:00:05Z- handleDVL() - DVL msg epochMillisec: 1730869205.02 2024-11-06T05:00:05Z- handleDvl() - RDI_Pathfinder msg: 1730869205.02 sec; seqNo:1297; #beams:4 2024-11-06T05:00:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.35 , 37.01 , 42.80 , 40.18 2024-11-06T05:00:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , -0.01 2024-11-06T05:00:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869205.05 epoch sec; seqNo:3682; 4061484.42 north; 594831.94 east; 36.69 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T05:00:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869202.58 2024-11-06T05:00:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:05Z- waiting for 1730869206.58, time is 1730869205.05 2024-11-06T05:00:05Z- AHRS_M2 msg: 1730869205.40 epoch sec; seqNo:3460 2024-11-06T05:00:05Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.43 psi 2024-11-06T05:00:05Z- handleDepth()- Depth_Keller msg: 1730869205.41 epoch sec; seqNo:3683; depth 50.17 2024-11-06T05:00:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869205.42 epoch sec; seqNo:3683; 4061484.42 north; 594831.94 east; 36.69 lat; -121.94 lon; 50.17 depth; utmZone: 10 2024-11-06T05:00:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869202.58 2024-11-06T05:00:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:05Z- waiting for 1730869206.58, time is 1730869205.42 2024-11-06T05:00:05Z- AHRS_M2 msg: 1730869205.80 epoch sec; seqNo:3461 2024-11-06T05:00:05Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.43 psi 2024-11-06T05:00:05Z- handleDepth()- Depth_Keller msg: 1730869205.81 epoch sec; seqNo:3684; depth 50.14 2024-11-06T05:00:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869205.83 epoch sec; seqNo:3684; 4061484.04 north; 594831.95 east; 36.69 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T05:00:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869202.58 2024-11-06T05:00:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:05Z- waiting for 1730869206.58, time is 1730869205.83 2024-11-06T05:00:06Z- AHRS_M2 msg: 1730869206.20 epoch sec; seqNo:3462 2024-11-06T05:00:06Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.43 psi 2024-11-06T05:00:06Z- handleDepth()- Depth_Keller msg: 1730869206.21 epoch sec; seqNo:3685; depth 50.10 2024-11-06T05:00:06Z- handleDVL() - DVL msg epochMillisec: 1730869206.21 2024-11-06T05:00:06Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:06Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:06Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:06Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:00:06Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:00:06Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:00:06Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:00:06Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:00:06Z- handleDvl() - RDI_Pathfinder msg: 1730869206.21 sec; seqNo:1298; #beams:0 2024-11-06T05:00:06Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:00:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , -0.01 2024-11-06T05:00:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869206.22 epoch sec; seqNo:3685; 4061483.66 north; 594831.95 east; 36.69 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T05:00:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869202.58 2024-11-06T05:00:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:06Z- waiting for 1730869206.58, time is 1730869206.22 2024-11-06T05:00:06Z- AHRS_M2 msg: 1730869206.61 epoch sec; seqNo:3463 2024-11-06T05:00:06Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.43 psi 2024-11-06T05:00:06Z- handleDepth()- Depth_Keller msg: 1730869206.62 epoch sec; seqNo:3686; depth 50.08 2024-11-06T05:00:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869206.62 epoch sec; seqNo:3686; 4061483.28 north; 594831.96 east; 36.69 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T05:00:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869202.58 2024-11-06T05:00:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:06Z- waiting for 1730869206.58, time is 1730869206.62 2024-11-06T05:00:06Z- LcmTrn::updateTrn() >>>> pose time 1730869206.62, meas time 1730869206.21 <<<< 2024-11-06T05:00:06Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869206.62 2024-11-06T05:00:06Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T05:00:06Z- TerrainNav::motionUpdate() - currEstimate = (4061483, 594831.96, 50.08) 2024-11-06T05:00:06Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:00:06Z- TerrainNav:: SubcloudNIS is 0.83, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:00:06Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T05:00:06Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T05:00:06Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:00:06Z- variances([0], [2], and psi_berg) = 0.890684 0.363686 0.000000 2024-11-06T05:00:06Z- publishEstimatesDecoder() - at 1730869206.62 sec 2024-11-06T05:00:06Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:00:06Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061428.86 594836.52 60.37 2024-11-06T05:00:06Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061426.52 594836.35 60.08 lat/lon: 36.69 -121.94 2024-11-06T05:00:06Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.89 0.36 1.22 0.00 2024-11-06T05:00:07Z- AHRS_M2 msg: 1730869207.01 epoch sec; seqNo:3464 2024-11-06T05:00:07Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.43 psi 2024-11-06T05:00:07Z- handleDepth()- Depth_Keller msg: 1730869207.02 epoch sec; seqNo:3687; depth 50.06 2024-11-06T05:00:07Z- handleDVL() - DVL msg epochMillisec: 1730869207.05 2024-11-06T05:00:07Z- handleDvl() - RDI_Pathfinder msg: 1730869207.05 sec; seqNo:1299; #beams:4 2024-11-06T05:00:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.54 , 37.01 , 43.17 , 40.00 2024-11-06T05:00:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , -0.06 2024-11-06T05:00:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869207.06 epoch sec; seqNo:3687; 4061482.90 north; 594831.96 east; 36.69 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T05:00:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869206.62 2024-11-06T05:00:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:07Z- waiting for 1730869210.62, time is 1730869207.06 2024-11-06T05:00:07Z- AHRS_M2 msg: 1730869207.42 epoch sec; seqNo:3465 2024-11-06T05:00:07Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.43 psi 2024-11-06T05:00:07Z- handleDepth()- Depth_Keller msg: 1730869207.42 epoch sec; seqNo:3688; depth 50.02 2024-11-06T05:00:07Z- handleDVL() - DVL msg epochMillisec: 1730869207.43 2024-11-06T05:00:07Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:07Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:07Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:07Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:00:07Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:00:07Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:00:07Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:00:07Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:00:07Z- handleDvl() - RDI_Pathfinder msg: 1730869207.43 sec; seqNo:1300; #beams:0 2024-11-06T05:00:07Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:00:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.00 , -0.06 2024-11-06T05:00:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869207.44 epoch sec; seqNo:3688; 4061482.52 north; 594831.97 east; 36.69 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T05:00:07Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869206.62 2024-11-06T05:00:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:07Z- waiting for 1730869210.62, time is 1730869207.44 2024-11-06T05:00:07Z- AHRS_M2 msg: 1730869207.82 epoch sec; seqNo:3466 2024-11-06T05:00:07Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.43 psi 2024-11-06T05:00:07Z- handleDepth()- Depth_Keller msg: 1730869207.84 epoch sec; seqNo:3689; depth 50.00 2024-11-06T05:00:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869207.84 epoch sec; seqNo:3689; 4061482.14 north; 594831.97 east; 36.69 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T05:00:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869206.62 2024-11-06T05:00:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:07Z- waiting for 1730869210.62, time is 1730869207.84 2024-11-06T05:00:08Z- AHRS_M2 msg: 1730869208.22 epoch sec; seqNo:3467 2024-11-06T05:00:08Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.43 psi 2024-11-06T05:00:08Z- handleDepth()- Depth_Keller msg: 1730869208.23 epoch sec; seqNo:3690; depth 49.97 2024-11-06T05:00:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869208.26 epoch sec; seqNo:3690; 4061481.75 north; 594830.75 east; 36.69 lat; -121.94 lon; 49.97 depth; utmZone: 10 2024-11-06T05:00:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869206.62 2024-11-06T05:00:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:08Z- waiting for 1730869210.62, time is 1730869208.26 2024-11-06T05:00:08Z- AHRS_M2 msg: 1730869208.63 epoch sec; seqNo:3468 2024-11-06T05:00:08Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.43 psi 2024-11-06T05:00:08Z- handleDepth()- Depth_Keller msg: 1730869208.63 epoch sec; seqNo:3691; depth 49.96 2024-11-06T05:00:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869208.64 epoch sec; seqNo:3691; 4061481.37 north; 594830.76 east; 36.69 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T05:00:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869206.62 2024-11-06T05:00:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:08Z- waiting for 1730869210.62, time is 1730869208.64 2024-11-06T05:00:09Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T05:00:09Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T05:00:09Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T05:00:09Z- AHRS_M2 msg: 1730869209.03 epoch sec; seqNo:3469 2024-11-06T05:00:09Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.43 psi 2024-11-06T05:00:09Z- handleDepth()- Depth_Keller msg: 1730869209.06 epoch sec; seqNo:3692; depth 49.93 2024-11-06T05:00:09Z- handleDVL() - DVL msg epochMillisec: 1730869209.09 2024-11-06T05:00:09Z- handleDvl() - RDI_Pathfinder msg: 1730869209.09 sec; seqNo:1301; #beams:4 2024-11-06T05:00:09Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.72 , 36.82 , 43.17 , 39.62 2024-11-06T05:00:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.11 , -0.09 2024-11-06T05:00:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869209.10 epoch sec; seqNo:3692; 4061480.99 north; 594830.76 east; 36.69 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T05:00:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869206.62 2024-11-06T05:00:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:09Z- waiting for 1730869210.62, time is 1730869209.10 2024-11-06T05:00:09Z- handleDepth()- Depth_Keller msg: 1730869209.46 epoch sec; seqNo:3693; depth 49.91 2024-11-06T05:00:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869209.48 epoch sec; seqNo:3693; 4061480.61 north; 594830.77 east; 36.69 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T05:00:09Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:00:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869206.62 2024-11-06T05:00:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:09Z- waiting for 1730869210.62, time is 1730869209.48 2024-11-06T05:00:09Z- AHRS_M2 msg: 1730869209.84 epoch sec; seqNo:3470 2024-11-06T05:00:09Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.42 psi 2024-11-06T05:00:09Z- handleDepth()- Depth_Keller msg: 1730869209.85 epoch sec; seqNo:3694; depth 49.89 2024-11-06T05:00:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869209.86 epoch sec; seqNo:3694; 4061480.24 north; 594830.77 east; 36.69 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T05:00:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869206.62 2024-11-06T05:00:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:10Z- waiting for 1730869210.62, time is 1730869209.86 2024-11-06T05:00:10Z- AHRS_M2 msg: 1730869210.24 epoch sec; seqNo:3471 2024-11-06T05:00:10Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.42 psi 2024-11-06T05:00:10Z- handleDepth()- Depth_Keller msg: 1730869210.25 epoch sec; seqNo:3695; depth 49.89 2024-11-06T05:00:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869210.26 epoch sec; seqNo:3695; 4061479.86 north; 594830.77 east; 36.69 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T05:00:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869206.62 2024-11-06T05:00:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:10Z- waiting for 1730869210.62, time is 1730869210.26 2024-11-06T05:00:10Z- AHRS_M2 msg: 1730869210.65 epoch sec; seqNo:3472 2024-11-06T05:00:10Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.42 psi 2024-11-06T05:00:10Z- handleDepth()- Depth_Keller msg: 1730869210.66 epoch sec; seqNo:3696; depth 49.86 2024-11-06T05:00:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869210.67 epoch sec; seqNo:3696; 4061479.48 north; 594830.78 east; 36.69 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T05:00:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869206.62 2024-11-06T05:00:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:10Z- waiting for 1730869210.62, time is 1730869210.67 2024-11-06T05:00:10Z- LcmTrn::updateTrn() >>>> pose time 1730869210.67, meas time 1730869209.09 <<<< 2024-11-06T05:00:10Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869210.67 2024-11-06T05:00:10Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869209.09 sec, ping # 0; waiting for INS data more recent than 1730869206.62... 2024-11-06T05:00:10Z- TerrainNav::motionUpdate() - currEstimate = (4061479, 594830.78, 49.86) 2024-11-06T05:00:10Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:00:10Z- TerrainNav:: SubcloudNIS is 0.83, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:00:10Z- TNavPF::Projecting Measurements 2024-11-06T05:00:10Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:00:10Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:00:10Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:00:10Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:00:10Z- TNavFilter::4 good beams. 2024-11-06T05:00:10Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:00:10Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:00:10Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:00:10Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:00:10Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:00:10Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:00:10Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:00:10Z- TerrainMapOctree: (vehN, vehE ) = (4061480.97, 594831.24). 2024-11-06T05:00:10Z- TerrainMapOctree: Min Distance = 244.84. 2024-11-06T05:00:10Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:00:10Z- Forcing Subcloud Comparison 2024-11-06T05:00:10Z- Weighting particles with subcloud comparison 2024-11-06T05:00:10Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:10Z- meanExpectedMeasDiff: 1.321768 delta_rms_squared: 2.353245 alpha: 0.991573 2024-11-06T05:00:10Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:10Z- meanExpectedMeasDiff: 0.309470 delta_rms_squared: 1.689175 alpha: 0.988298 2024-11-06T05:00:10Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:10Z- meanExpectedMeasDiff: 1.793464 delta_rms_squared: 1.892970 alpha: 0.989545 2024-11-06T05:00:10Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:10Z- meanExpectedMeasDiff: -0.087351 delta_rms_squared: 1.886230 alpha: 0.989508 2024-11-06T05:00:10Z- Subcloud NIS: 0.802790 2024-11-06T05:00:10Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:00:10Z- TNavPF:: sumWeights = 0.433810 2024-11-06T05:00:10Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:00:10Z- TNavPF::First Term of Map Covariance Matrix is 0.46 2024-11-06T05:00:10Z- TNavPF::Calculated NIS Value : 0.14 numBeams normalized NIS: 0.03 2024-11-06T05:00:10Z- TerrainNav::Resampling particle filter... 2024-11-06T05:00:10Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869209.09 sec. 2024-11-06T05:00:10Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:00:10Z- variances([0], [2], and psi_berg) = 0.835515 0.348065 0.000000 2024-11-06T05:00:10Z- publishEstimatesDecoder() - at 1730869210.67 sec 2024-11-06T05:00:10Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:00:10Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061425.10 594835.33 60.26 2024-11-06T05:00:10Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061422.74 594835.21 59.42 lat/lon: 36.69 -121.94 2024-11-06T05:00:10Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.84 0.35 0.79 0.00 2024-11-06T05:00:11Z- AHRS_M2 msg: 1730869211.07 epoch sec; seqNo:3473 2024-11-06T05:00:11Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.41 psi 2024-11-06T05:00:11Z- handleDepth()- Depth_Keller msg: 1730869211.07 epoch sec; seqNo:3697; depth 49.85 2024-11-06T05:00:11Z- handleDVL() - DVL msg epochMillisec: 1730869211.11 2024-11-06T05:00:11Z- handleDvl() - RDI_Pathfinder msg: 1730869211.11 sec; seqNo:1302; #beams:4 2024-11-06T05:00:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.35 , 37.19 , 42.99 , 39.44 2024-11-06T05:00:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.22 , -0.02 2024-11-06T05:00:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869211.12 epoch sec; seqNo:3697; 4061479.48 north; 594830.78 east; 36.69 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T05:00:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869210.67 2024-11-06T05:00:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:11Z- waiting for 1730869214.67, time is 1730869211.12 2024-11-06T05:00:11Z- AHRS_M2 msg: 1730869211.46 epoch sec; seqNo:3474 2024-11-06T05:00:11Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.41 psi 2024-11-06T05:00:11Z- handleDepth()- Depth_Keller msg: 1730869211.46 epoch sec; seqNo:3698; depth 49.85 2024-11-06T05:00:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869211.48 epoch sec; seqNo:3698; 4061479.09 north; 594829.56 east; 36.69 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T05:00:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869210.67 2024-11-06T05:00:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:11Z- waiting for 1730869214.67, time is 1730869211.48 2024-11-06T05:00:11Z- AHRS_M2 msg: 1730869211.86 epoch sec; seqNo:3475 2024-11-06T05:00:11Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.40 psi 2024-11-06T05:00:11Z- handleDepth()- Depth_Keller msg: 1730869211.87 epoch sec; seqNo:3699; depth 49.84 2024-11-06T05:00:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869211.89 epoch sec; seqNo:3699; 4061478.71 north; 594829.57 east; 36.69 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T05:00:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869210.67 2024-11-06T05:00:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:12Z- waiting for 1730869214.67, time is 1730869211.89 2024-11-06T05:00:12Z- AHRS_M2 msg: 1730869212.26 epoch sec; seqNo:3476 2024-11-06T05:00:12Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.40 psi 2024-11-06T05:00:12Z- handleDepth()- Depth_Keller msg: 1730869212.27 epoch sec; seqNo:3700; depth 49.85 2024-11-06T05:00:12Z- handleDVL() - DVL msg epochMillisec: 1730869212.28 2024-11-06T05:00:12Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:12Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:12Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:12Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:00:12Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:00:12Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:00:12Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:00:12Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:00:12Z- handleDvl() - RDI_Pathfinder msg: 1730869212.28 sec; seqNo:1303; #beams:0 2024-11-06T05:00:12Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:00:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.22 , -0.02 2024-11-06T05:00:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869212.30 epoch sec; seqNo:3700; 4061478.33 north; 594829.57 east; 36.69 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T05:00:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869210.67 2024-11-06T05:00:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:12Z- waiting for 1730869214.67, time is 1730869212.30 2024-11-06T05:00:12Z- AHRS_M2 msg: 1730869212.68 epoch sec; seqNo:3477 2024-11-06T05:00:12Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 3.39 psi 2024-11-06T05:00:12Z- handleDepth()- Depth_Keller msg: 1730869212.70 epoch sec; seqNo:3701; depth 49.83 2024-11-06T05:00:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869212.75 epoch sec; seqNo:3701; 4061477.95 north; 594829.58 east; 36.69 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T05:00:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869210.67 2024-11-06T05:00:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:12Z- waiting for 1730869214.67, time is 1730869212.75 2024-11-06T05:00:13Z- AHRS_M2 msg: 1730869213.08 epoch sec; seqNo:3478 2024-11-06T05:00:13Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 3.39 psi 2024-11-06T05:00:13Z- handleDepth()- Depth_Keller msg: 1730869213.08 epoch sec; seqNo:3702; depth 49.83 2024-11-06T05:00:13Z- handleDVL() - DVL msg epochMillisec: 1730869213.10 2024-11-06T05:00:13Z- handleDvl() - RDI_Pathfinder msg: 1730869213.10 sec; seqNo:1304; #beams:4 2024-11-06T05:00:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.35 , 37.38 , 42.99 , 39.62 2024-11-06T05:00:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.16 , 0.12 2024-11-06T05:00:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869213.12 epoch sec; seqNo:3702; 4061477.57 north; 594829.58 east; 36.69 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T05:00:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869210.67 2024-11-06T05:00:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:13Z- waiting for 1730869214.67, time is 1730869213.12 2024-11-06T05:00:13Z- AHRS_M2 msg: 1730869213.48 epoch sec; seqNo:3479 2024-11-06T05:00:13Z- AHRS_M2 msg: 0.15 phi; 0.04 theta; 3.39 psi 2024-11-06T05:00:13Z- handleDepth()- Depth_Keller msg: 1730869213.49 epoch sec; seqNo:3703; depth 49.81 2024-11-06T05:00:13Z- handleDVL() - DVL msg epochMillisec: 1730869213.49 2024-11-06T05:00:13Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:13Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:13Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:13Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:00:13Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:00:13Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:00:13Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:00:13Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:00:13Z- handleDvl() - RDI_Pathfinder msg: 1730869213.49 sec; seqNo:1305; #beams:0 2024-11-06T05:00:13Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:00:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.16 , 0.12 2024-11-06T05:00:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869213.51 epoch sec; seqNo:3703; 4061477.19 north; 594829.58 east; 36.69 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T05:00:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869210.67 2024-11-06T05:00:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:13Z- waiting for 1730869214.67, time is 1730869213.51 2024-11-06T05:00:13Z- AHRS_M2 msg: 1730869213.88 epoch sec; seqNo:3480 2024-11-06T05:00:13Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.39 psi 2024-11-06T05:00:13Z- handleDepth()- Depth_Keller msg: 1730869213.89 epoch sec; seqNo:3704; depth 49.82 2024-11-06T05:00:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869213.91 epoch sec; seqNo:3704; 4061476.81 north; 594829.59 east; 36.69 lat; -121.94 lon; 49.82 depth; utmZone: 10 2024-11-06T05:00:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869210.67 2024-11-06T05:00:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:14Z- waiting for 1730869214.67, time is 1730869213.91 2024-11-06T05:00:14Z- AHRS_M2 msg: 1730869214.28 epoch sec; seqNo:3481 2024-11-06T05:00:14Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 3.39 psi 2024-11-06T05:00:14Z- handleDepth()- Depth_Keller msg: 1730869214.30 epoch sec; seqNo:3705; depth 49.83 2024-11-06T05:00:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869214.31 epoch sec; seqNo:3705; 4061476.81 north; 594829.59 east; 36.69 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T05:00:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869210.67 2024-11-06T05:00:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:14Z- waiting for 1730869214.67, time is 1730869214.31 2024-11-06T05:00:14Z- AHRS_M2 msg: 1730869214.72 epoch sec; seqNo:3482 2024-11-06T05:00:14Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 3.39 psi 2024-11-06T05:00:14Z- handleDepth()- Depth_Keller msg: 1730869214.75 epoch sec; seqNo:3706; depth 49.80 2024-11-06T05:00:14Z- handleDVL() - DVL msg epochMillisec: 1730869214.77 2024-11-06T05:00:14Z- handleDvl() - RDI_Pathfinder msg: 1730869214.77 sec; seqNo:1306; #beams:4 2024-11-06T05:00:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.72 , 37.94 , 43.73 , 40.00 2024-11-06T05:00:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , 0.15 2024-11-06T05:00:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869214.78 epoch sec; seqNo:3706; 4061476.04 north; 594828.38 east; 36.69 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T05:00:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869210.67 2024-11-06T05:00:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:14Z- waiting for 1730869214.67, time is 1730869214.78 2024-11-06T05:00:14Z- LcmTrn::updateTrn() >>>> pose time 1730869214.78, meas time 1730869214.77 <<<< 2024-11-06T05:00:14Z- LcmTrn::updateTrn() equating pose 1730869214.77 and meas times 1730869214.77 2024-11-06T05:00:14Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869214.77 2024-11-06T05:00:14Z- TerrainNav::motionUpdate() - currEstimate = (4061476, 594828.38, 49.80) 2024-11-06T05:00:14Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:00:14Z- TerrainNav:: SubcloudNIS is 0.80, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:00:14Z- TNavPF::Projecting Measurements 2024-11-06T05:00:14Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:00:14Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:00:14Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:00:14Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:00:14Z- TNavFilter::4 good beams. 2024-11-06T05:00:14Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:00:14Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:00:14Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:00:14Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:00:14Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:00:14Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:00:14Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:00:14Z- TerrainMapOctree: (vehN, vehE ) = (4061476.04, 594828.38). 2024-11-06T05:00:14Z- TerrainMapOctree: Min Distance = 239.24. 2024-11-06T05:00:14Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:00:15Z- Forcing Subcloud Comparison 2024-11-06T05:00:15Z- Weighting particles with subcloud comparison 2024-11-06T05:00:15Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:15Z- meanExpectedMeasDiff: 1.748546 delta_rms_squared: 1.664225 alpha: 0.988125 2024-11-06T05:00:15Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:15Z- meanExpectedMeasDiff: 0.866658 delta_rms_squared: 1.184472 alpha: 0.983395 2024-11-06T05:00:15Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:15Z- meanExpectedMeasDiff: 2.791064 delta_rms_squared: 1.529006 alpha: 0.987088 2024-11-06T05:00:15Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:15Z- meanExpectedMeasDiff: -0.492085 delta_rms_squared: 1.335239 alpha: 0.985242 2024-11-06T05:00:15Z- Subcloud NIS: 0.760545 2024-11-06T05:00:15Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:00:15Z- TNavPF:: sumWeights = 0.278500 2024-11-06T05:00:15Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:00:15Z- TNavPF::First Term of Map Covariance Matrix is 0.37 2024-11-06T05:00:15Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T05:00:15Z- TerrainNav::Resampling particle filter... 2024-11-06T05:00:15Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869214.77 sec, ping # 0. 2024-11-06T05:00:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:00:15Z- variances([0], [2], and psi_berg) = 0.857422 0.370268 0.000000 2024-11-06T05:00:15Z- publishEstimatesDecoder() - at 1730869214.77 sec 2024-11-06T05:00:15Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:00:15Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061421.45 594831.97 59.32 2024-11-06T05:00:15Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061419.30 594832.81 58.81 lat/lon: 36.69 -121.94 2024-11-06T05:00:15Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.86 0.37 0.47 0.00 2024-11-06T05:00:15Z- AHRS_M2 msg: 1730869215.09 epoch sec; seqNo:3483 2024-11-06T05:00:15Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 3.39 psi 2024-11-06T05:00:15Z- handleDepth()- Depth_Keller msg: 1730869215.10 epoch sec; seqNo:3707; depth 49.79 2024-11-06T05:00:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869215.14 epoch sec; seqNo:3707; 4061476.04 north; 594828.38 east; 36.69 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T05:00:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869214.77 2024-11-06T05:00:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:15Z- waiting for 1730869218.77, time is 1730869215.14 2024-11-06T05:00:15Z- AHRS_M2 msg: 1730869215.49 epoch sec; seqNo:3484 2024-11-06T05:00:15Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 3.39 psi 2024-11-06T05:00:15Z- handleDepth()- Depth_Keller msg: 1730869215.50 epoch sec; seqNo:3708; depth 49.79 2024-11-06T05:00:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869215.52 epoch sec; seqNo:3708; 4061475.66 north; 594828.38 east; 36.69 lat; -121.94 lon; 49.79 depth; utmZone: 10 2024-11-06T05:00:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869214.77 2024-11-06T05:00:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:15Z- waiting for 1730869218.77, time is 1730869215.52 2024-11-06T05:00:15Z- AHRS_M2 msg: 1730869215.90 epoch sec; seqNo:3485 2024-11-06T05:00:15Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 3.39 psi 2024-11-06T05:00:15Z- handleDepth()- Depth_Keller msg: 1730869215.91 epoch sec; seqNo:3709; depth 49.78 2024-11-06T05:00:15Z- handleDVL() - DVL msg epochMillisec: 1730869215.91 2024-11-06T05:00:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:00:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:00:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:00:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:00:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:00:15Z- handleDvl() - RDI_Pathfinder msg: 1730869215.91 sec; seqNo:1307; #beams:0 2024-11-06T05:00:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:00:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , 0.15 2024-11-06T05:00:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869215.92 epoch sec; seqNo:3709; 4061475.28 north; 594828.38 east; 36.69 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T05:00:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869214.77 2024-11-06T05:00:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:16Z- waiting for 1730869218.77, time is 1730869215.92 2024-11-06T05:00:16Z- AHRS_M2 msg: 1730869216.32 epoch sec; seqNo:3486 2024-11-06T05:00:16Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 3.39 psi 2024-11-06T05:00:16Z- handleDepth()- Depth_Keller msg: 1730869216.33 epoch sec; seqNo:3710; depth 49.77 2024-11-06T05:00:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869216.34 epoch sec; seqNo:3710; 4061474.91 north; 594828.39 east; 36.69 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T05:00:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869214.77 2024-11-06T05:00:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:16Z- waiting for 1730869218.77, time is 1730869216.34 2024-11-06T05:00:16Z- AHRS_M2 msg: 1730869216.74 epoch sec; seqNo:3487 2024-11-06T05:00:16Z- AHRS_M2 msg: 0.14 phi; 0.03 theta; 3.39 psi 2024-11-06T05:00:16Z- handleDepth()- Depth_Keller msg: 1730869216.75 epoch sec; seqNo:3711; depth 49.75 2024-11-06T05:00:16Z- handleDVL() - DVL msg epochMillisec: 1730869216.77 2024-11-06T05:00:16Z- handleDvl() - RDI_Pathfinder msg: 1730869216.77 sec; seqNo:1308; #beams:4 2024-11-06T05:00:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 38.33 , 43.43 , 40.37 2024-11-06T05:00:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.12 , 0.02 2024-11-06T05:00:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869216.80 epoch sec; seqNo:3711; 4061474.53 north; 594828.39 east; 36.69 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T05:00:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869214.77 2024-11-06T05:00:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:16Z- waiting for 1730869218.77, time is 1730869216.80 2024-11-06T05:00:17Z- AHRS_M2 msg: 1730869217.11 epoch sec; seqNo:3488 2024-11-06T05:00:17Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.39 psi 2024-11-06T05:00:17Z- handleDepth()- Depth_Keller msg: 1730869217.12 epoch sec; seqNo:3712; depth 49.73 2024-11-06T05:00:17Z- handleDVL() - DVL msg epochMillisec: 1730869217.12 2024-11-06T05:00:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:00:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:00:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:00:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:00:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:00:17Z- handleDvl() - RDI_Pathfinder msg: 1730869217.12 sec; seqNo:1309; #beams:0 2024-11-06T05:00:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:00:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.12 , 0.02 2024-11-06T05:00:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869217.13 epoch sec; seqNo:3712; 4061474.15 north; 594828.40 east; 36.69 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T05:00:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869214.77 2024-11-06T05:00:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:17Z- waiting for 1730869218.77, time is 1730869217.13 2024-11-06T05:00:17Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T05:00:17Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T05:00:17Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T05:00:17Z- AHRS_M2 msg: 1730869217.51 epoch sec; seqNo:3489 2024-11-06T05:00:17Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.39 psi 2024-11-06T05:00:17Z- handleDepth()- Depth_Keller msg: 1730869217.54 epoch sec; seqNo:3713; depth 49.73 2024-11-06T05:00:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869217.55 epoch sec; seqNo:3713; 4061473.77 north; 594828.40 east; 36.69 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T05:00:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869214.77 2024-11-06T05:00:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:17Z- waiting for 1730869218.77, time is 1730869217.55 2024-11-06T05:00:17Z- AHRS_M2 msg: 1730869217.92 epoch sec; seqNo:3490 2024-11-06T05:00:17Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.40 psi 2024-11-06T05:00:17Z- handleDepth()- Depth_Keller msg: 1730869217.92 epoch sec; seqNo:3714; depth 49.70 2024-11-06T05:00:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869217.94 epoch sec; seqNo:3714; 4061473.39 north; 594828.41 east; 36.69 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T05:00:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869214.77 2024-11-06T05:00:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:18Z- waiting for 1730869218.77, time is 1730869217.94 2024-11-06T05:00:18Z- AHRS_M2 msg: 1730869218.32 epoch sec; seqNo:3491 2024-11-06T05:00:18Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.40 psi 2024-11-06T05:00:18Z- handleDepth()- Depth_Keller msg: 1730869218.33 epoch sec; seqNo:3715; depth 49.70 2024-11-06T05:00:18Z- handleDVL() - DVL msg epochMillisec: 1730869218.34 2024-11-06T05:00:18Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:18Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:18Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:18Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:00:18Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:00:18Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:00:18Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:00:18Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:00:18Z- handleDvl() - RDI_Pathfinder msg: 1730869218.34 sec; seqNo:1310; #beams:0 2024-11-06T05:00:18Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:00:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.12 , 0.02 2024-11-06T05:00:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869218.36 epoch sec; seqNo:3715; 4061473.01 north; 594828.41 east; 36.69 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T05:00:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869214.77 2024-11-06T05:00:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:18Z- waiting for 1730869218.77, time is 1730869218.36 2024-11-06T05:00:18Z- AHRS_M2 msg: 1730869218.75 epoch sec; seqNo:3492 2024-11-06T05:00:18Z- AHRS_M2 msg: 0.14 phi; 0.02 theta; 3.40 psi 2024-11-06T05:00:18Z- handleDepth()- Depth_Keller msg: 1730869218.75 epoch sec; seqNo:3716; depth 49.66 2024-11-06T05:00:18Z- handleDVL() - DVL msg epochMillisec: 1730869218.78 2024-11-06T05:00:18Z- handleDvl() - RDI_Pathfinder msg: 1730869218.78 sec; seqNo:1311; #beams:4 2024-11-06T05:00:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 38.13 , 43.22 , 40.37 2024-11-06T05:00:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.04 , -0.11 2024-11-06T05:00:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869218.79 epoch sec; seqNo:3716; 4061472.63 north; 594828.41 east; 36.69 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T05:00:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869214.77 2024-11-06T05:00:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:18Z- waiting for 1730869218.77, time is 1730869218.79 2024-11-06T05:00:18Z- LcmTrn::updateTrn() >>>> pose time 1730869218.79, meas time 1730869218.78 <<<< 2024-11-06T05:00:18Z- LcmTrn::updateTrn() equating pose 1730869218.78 and meas times 1730869218.78 2024-11-06T05:00:18Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869218.78 2024-11-06T05:00:18Z- TerrainNav::motionUpdate() - currEstimate = (4061473, 594828.41, 49.66) 2024-11-06T05:00:18Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:00:18Z- TerrainNav:: SubcloudNIS is 0.76, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:00:18Z- TNavPF::Projecting Measurements 2024-11-06T05:00:18Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:00:18Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:00:18Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:00:18Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:00:18Z- TNavFilter::4 good beams. 2024-11-06T05:00:18Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:00:18Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:00:18Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:00:18Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:00:18Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:00:18Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:00:18Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:00:18Z- TerrainMapOctree: (vehN, vehE ) = (4061472.63, 594828.41). 2024-11-06T05:00:18Z- TerrainMapOctree: Min Distance = 236.04. 2024-11-06T05:00:18Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:00:19Z- Forcing Subcloud Comparison 2024-11-06T05:00:19Z- Weighting particles with subcloud comparison 2024-11-06T05:00:19Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:19Z- meanExpectedMeasDiff: 1.601107 delta_rms_squared: 1.214059 alpha: 0.983793 2024-11-06T05:00:19Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:19Z- meanExpectedMeasDiff: 0.227196 delta_rms_squared: 0.826096 alpha: 0.976362 2024-11-06T05:00:19Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:19Z- meanExpectedMeasDiff: 1.210501 delta_rms_squared: 1.689316 alpha: 0.988299 2024-11-06T05:00:19Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:19Z- meanExpectedMeasDiff: -1.285384 delta_rms_squared: 0.944808 alpha: 0.979270 2024-11-06T05:00:19Z- Subcloud NIS: 0.706263 2024-11-06T05:00:19Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:00:19Z- TNavPF:: sumWeights = 0.409248 2024-11-06T05:00:19Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:00:19Z- TNavPF::First Term of Map Covariance Matrix is 0.54 2024-11-06T05:00:19Z- TNavPF::Calculated NIS Value : 0.22 numBeams normalized NIS: 0.06 2024-11-06T05:00:19Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869218.78 sec, ping # 0. 2024-11-06T05:00:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:00:19Z- variances([0], [2], and psi_berg) = 0.829978 0.361129 0.000000 2024-11-06T05:00:19Z- publishEstimatesDecoder() - at 1730869218.78 sec 2024-11-06T05:00:19Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:00:19Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061413.78 594834.94 58.60 2024-11-06T05:00:19Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061415.81 594832.87 58.50 lat/lon: 36.69 -121.94 2024-11-06T05:00:19Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.83 0.36 0.38 0.00 2024-11-06T05:00:19Z- handleDepth()- Depth_Keller msg: 1730869219.15 epoch sec; seqNo:3717; depth 49.64 2024-11-06T05:00:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869219.18 epoch sec; seqNo:3717; 4061472.25 north; 594828.42 east; 36.69 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T05:00:19Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:00:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869218.78 2024-11-06T05:00:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:19Z- waiting for 1730869222.78, time is 1730869219.18 2024-11-06T05:00:19Z- handleDepth()- Depth_Keller msg: 1730869219.58 epoch sec; seqNo:3718; depth 49.60 2024-11-06T05:00:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869219.60 epoch sec; seqNo:3718; 4061471.87 north; 594828.42 east; 36.69 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T05:00:19Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:00:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869218.78 2024-11-06T05:00:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:19Z- waiting for 1730869222.78, time is 1730869219.60 2024-11-06T05:00:19Z- AHRS_M2 msg: 1730869219.94 epoch sec; seqNo:3493 2024-11-06T05:00:19Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.39 psi 2024-11-06T05:00:19Z- handleDepth()- Depth_Keller msg: 1730869219.95 epoch sec; seqNo:3719; depth 49.59 2024-11-06T05:00:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869219.95 epoch sec; seqNo:3719; 4061471.50 north; 594828.43 east; 36.69 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T05:00:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869218.78 2024-11-06T05:00:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:20Z- waiting for 1730869222.78, time is 1730869219.95 2024-11-06T05:00:20Z- AHRS_M2 msg: 1730869220.34 epoch sec; seqNo:3494 2024-11-06T05:00:20Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.39 psi 2024-11-06T05:00:20Z- handleDepth()- Depth_Keller msg: 1730869220.35 epoch sec; seqNo:3720; depth 49.57 2024-11-06T05:00:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869220.36 epoch sec; seqNo:3720; 4061471.12 north; 594828.43 east; 36.69 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T05:00:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869218.78 2024-11-06T05:00:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:20Z- waiting for 1730869222.78, time is 1730869220.36 2024-11-06T05:00:20Z- AHRS_M2 msg: 1730869220.76 epoch sec; seqNo:3495 2024-11-06T05:00:20Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.39 psi 2024-11-06T05:00:20Z- handleDepth()- Depth_Keller msg: 1730869220.76 epoch sec; seqNo:3721; depth 49.55 2024-11-06T05:00:20Z- handleDVL() - DVL msg epochMillisec: 1730869220.78 2024-11-06T05:00:20Z- handleDvl() - RDI_Pathfinder msg: 1730869220.78 sec; seqNo:1312; #beams:4 2024-11-06T05:00:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 37.72 , 42.82 , 40.37 2024-11-06T05:00:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.11 , -0.12 2024-11-06T05:00:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869220.80 epoch sec; seqNo:3721; 4061470.74 north; 594828.43 east; 36.69 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T05:00:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869218.78 2024-11-06T05:00:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:20Z- waiting for 1730869222.78, time is 1730869220.80 2024-11-06T05:00:21Z- AHRS_M2 msg: 1730869221.15 epoch sec; seqNo:3496 2024-11-06T05:00:21Z- AHRS_M2 msg: 0.14 phi; 0.01 theta; 3.39 psi 2024-11-06T05:00:21Z- handleDepth()- Depth_Keller msg: 1730869221.16 epoch sec; seqNo:3722; depth 49.52 2024-11-06T05:00:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869221.18 epoch sec; seqNo:3722; 4061470.35 north; 594827.22 east; 36.69 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T05:00:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869218.78 2024-11-06T05:00:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:21Z- waiting for 1730869222.78, time is 1730869221.18 2024-11-06T05:00:21Z- AHRS_M2 msg: 1730869221.56 epoch sec; seqNo:3497 2024-11-06T05:00:21Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.38 psi 2024-11-06T05:00:21Z- handleDepth()- Depth_Keller msg: 1730869221.57 epoch sec; seqNo:3723; depth 49.50 2024-11-06T05:00:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869221.58 epoch sec; seqNo:3723; 4061469.97 north; 594827.22 east; 36.69 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T05:00:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869218.78 2024-11-06T05:00:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:21Z- waiting for 1730869222.78, time is 1730869221.58 2024-11-06T05:00:21Z- AHRS_M2 msg: 1730869221.96 epoch sec; seqNo:3498 2024-11-06T05:00:21Z- AHRS_M2 msg: 0.15 phi; 0.01 theta; 3.38 psi 2024-11-06T05:00:21Z- handleDepth()- Depth_Keller msg: 1730869221.96 epoch sec; seqNo:3724; depth 49.47 2024-11-06T05:00:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869221.97 epoch sec; seqNo:3724; 4061469.59 north; 594827.23 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T05:00:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869218.78 2024-11-06T05:00:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:22Z- waiting for 1730869222.78, time is 1730869221.97 2024-11-06T05:00:22Z- AHRS_M2 msg: 1730869222.36 epoch sec; seqNo:3499 2024-11-06T05:00:22Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.38 psi 2024-11-06T05:00:22Z- handleDepth()- Depth_Keller msg: 1730869222.37 epoch sec; seqNo:3725; depth 49.47 2024-11-06T05:00:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869222.38 epoch sec; seqNo:3725; 4061469.21 north; 594827.23 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T05:00:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869218.78 2024-11-06T05:00:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:22Z- waiting for 1730869222.78, time is 1730869222.38 2024-11-06T05:00:22Z- handleDepth()- Depth_Keller msg: 1730869222.79 epoch sec; seqNo:3726; depth 49.45 2024-11-06T05:00:22Z- handleDVL() - DVL msg epochMillisec: 1730869222.81 2024-11-06T05:00:22Z- handleDvl() - RDI_Pathfinder msg: 1730869222.81 sec; seqNo:1313; #beams:4 2024-11-06T05:00:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 37.72 , 42.61 , 40.57 2024-11-06T05:00:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.20 , 0.00 2024-11-06T05:00:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869222.83 epoch sec; seqNo:3726; 4061468.83 north; 594827.24 east; 36.69 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T05:00:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869218.78 2024-11-06T05:00:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:22Z- waiting for 1730869222.78, time is 1730869222.83 2024-11-06T05:00:22Z- LcmTrn::updateTrn() >>>> pose time 1730869222.83, meas time 1730869222.81 <<<< 2024-11-06T05:00:22Z- LcmTrn::updateTrn() equating pose 1730869222.81 and meas times 1730869222.81 2024-11-06T05:00:22Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869222.81 2024-11-06T05:00:22Z- TerrainNav::motionUpdate() - currEstimate = (4061469, 594827.24, 49.45) 2024-11-06T05:00:22Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:00:22Z- TerrainNav:: SubcloudNIS is 0.71, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:00:22Z- TNavPF::Projecting Measurements 2024-11-06T05:00:22Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:00:22Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:00:22Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:00:22Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:00:22Z- TNavFilter::4 good beams. 2024-11-06T05:00:22Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:00:22Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:00:22Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:00:22Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:00:22Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:00:22Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:00:22Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:00:22Z- TerrainMapOctree: (vehN, vehE ) = (4061468.83, 594827.24). 2024-11-06T05:00:22Z- TerrainMapOctree: Min Distance = 232.06. 2024-11-06T05:00:22Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:00:23Z- Forcing Subcloud Comparison 2024-11-06T05:00:23Z- Weighting particles with subcloud comparison 2024-11-06T05:00:23Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:23Z- meanExpectedMeasDiff: 0.662510 delta_rms_squared: 1.333195 alpha: 0.985220 2024-11-06T05:00:23Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:23Z- meanExpectedMeasDiff: -0.397648 delta_rms_squared: 0.695550 alpha: 0.972049 2024-11-06T05:00:23Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:23Z- meanExpectedMeasDiff: 0.571956 delta_rms_squared: 0.911540 alpha: 0.978530 2024-11-06T05:00:23Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:23Z- meanExpectedMeasDiff: -1.881189 delta_rms_squared: 0.817899 alpha: 0.976131 2024-11-06T05:00:23Z- Subcloud NIS: 0.647396 2024-11-06T05:00:23Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:00:23Z- TNavPF:: sumWeights = 0.461013 2024-11-06T05:00:23Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:00:23Z- TNavPF::First Term of Map Covariance Matrix is 0.44 2024-11-06T05:00:23Z- TNavPF::Calculated NIS Value : 0.27 numBeams normalized NIS: 0.07 2024-11-06T05:00:23Z- TerrainNav::Resampling particle filter... 2024-11-06T05:00:23Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869222.81 sec, ping # 0. 2024-11-06T05:00:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:00:23Z- variances([0], [2], and psi_berg) = 0.821176 0.366934 0.000000 2024-11-06T05:00:23Z- publishEstimatesDecoder() - at 1730869222.81 sec 2024-11-06T05:00:23Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:00:23Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061414.34 594830.80 58.47 2024-11-06T05:00:23Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061412.04 594831.71 58.42 lat/lon: 36.69 -121.94 2024-11-06T05:00:23Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.82 0.37 0.35 0.00 2024-11-06T05:00:23Z- AHRS_M2 msg: 1730869223.18 epoch sec; seqNo:3500 2024-11-06T05:00:23Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.37 psi 2024-11-06T05:00:23Z- handleDepth()- Depth_Keller msg: 1730869223.20 epoch sec; seqNo:3727; depth 49.44 2024-11-06T05:00:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869223.23 epoch sec; seqNo:3727; 4061468.45 north; 594827.24 east; 36.69 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T05:00:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869222.81 2024-11-06T05:00:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:23Z- waiting for 1730869226.81, time is 1730869223.23 2024-11-06T05:00:23Z- AHRS_M2 msg: 1730869223.59 epoch sec; seqNo:3501 2024-11-06T05:00:23Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.36 psi 2024-11-06T05:00:23Z- handleDepth()- Depth_Keller msg: 1730869223.59 epoch sec; seqNo:3728; depth 49.44 2024-11-06T05:00:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869223.60 epoch sec; seqNo:3728; 4061468.45 north; 594827.24 east; 36.69 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T05:00:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869222.81 2024-11-06T05:00:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:23Z- waiting for 1730869226.81, time is 1730869223.60 2024-11-06T05:00:23Z- AHRS_M2 msg: 1730869223.98 epoch sec; seqNo:3502 2024-11-06T05:00:23Z- AHRS_M2 msg: 0.15 phi; 0.00 theta; 3.36 psi 2024-11-06T05:00:23Z- handleDepth()- Depth_Keller msg: 1730869223.98 epoch sec; seqNo:3729; depth 49.43 2024-11-06T05:00:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869224.00 epoch sec; seqNo:3729; 4061468.07 north; 594827.24 east; 36.69 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T05:00:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869222.81 2024-11-06T05:00:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:24Z- waiting for 1730869226.81, time is 1730869224.00 2024-11-06T05:00:24Z- AHRS_M2 msg: 1730869224.38 epoch sec; seqNo:3503 2024-11-06T05:00:24Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.36 psi 2024-11-06T05:00:24Z- handleDepth()- Depth_Keller msg: 1730869224.39 epoch sec; seqNo:3730; depth 49.43 2024-11-06T05:00:24Z- handleDVL() - DVL msg epochMillisec: 1730869224.39 2024-11-06T05:00:24Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:24Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:24Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:24Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:00:24Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:00:24Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:00:24Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:00:24Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:00:24Z- handleDvl() - RDI_Pathfinder msg: 1730869224.39 sec; seqNo:1314; #beams:0 2024-11-06T05:00:24Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:00:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.20 , 0.00 2024-11-06T05:00:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869224.40 epoch sec; seqNo:3730; 4061467.68 north; 594826.03 east; 36.69 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T05:00:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869222.81 2024-11-06T05:00:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:24Z- waiting for 1730869226.81, time is 1730869224.40 2024-11-06T05:00:24Z- AHRS_M2 msg: 1730869224.79 epoch sec; seqNo:3504 2024-11-06T05:00:24Z- AHRS_M2 msg: 0.15 phi; -0.00 theta; 3.36 psi 2024-11-06T05:00:24Z- handleDepth()- Depth_Keller msg: 1730869224.79 epoch sec; seqNo:3731; depth 49.44 2024-11-06T05:00:24Z- handleDVL() - DVL msg epochMillisec: 1730869224.83 2024-11-06T05:00:24Z- handleDvl() - RDI_Pathfinder msg: 1730869224.83 sec; seqNo:1315; #beams:4 2024-11-06T05:00:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 37.92 , 42.61 , 40.98 2024-11-06T05:00:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.16 , 0.10 2024-11-06T05:00:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869224.85 epoch sec; seqNo:3731; 4061467.30 north; 594826.03 east; 36.69 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T05:00:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869222.81 2024-11-06T05:00:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:24Z- waiting for 1730869226.81, time is 1730869224.85 2024-11-06T05:00:25Z- AHRS_M2 msg: 1730869225.19 epoch sec; seqNo:3505 2024-11-06T05:00:25Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 3.36 psi 2024-11-06T05:00:25Z- handleDepth()- Depth_Keller msg: 1730869225.20 epoch sec; seqNo:3732; depth 49.44 2024-11-06T05:00:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869225.21 epoch sec; seqNo:3732; 4061466.92 north; 594826.04 east; 36.69 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T05:00:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869222.81 2024-11-06T05:00:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:25Z- waiting for 1730869226.81, time is 1730869225.21 2024-11-06T05:00:25Z- AHRS_M2 msg: 1730869225.60 epoch sec; seqNo:3506 2024-11-06T05:00:25Z- AHRS_M2 msg: 0.16 phi; -0.00 theta; 3.35 psi 2024-11-06T05:00:25Z- handleDepth()- Depth_Keller msg: 1730869225.60 epoch sec; seqNo:3733; depth 49.45 2024-11-06T05:00:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869225.62 epoch sec; seqNo:3733; 4061466.54 north; 594826.04 east; 36.69 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T05:00:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869222.81 2024-11-06T05:00:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:25Z- waiting for 1730869226.81, time is 1730869225.62 2024-11-06T05:00:26Z- AHRS_M2 msg: 1730869226.00 epoch sec; seqNo:3507 2024-11-06T05:00:26Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.35 psi 2024-11-06T05:00:26Z- handleDepth()- Depth_Keller msg: 1730869226.00 epoch sec; seqNo:3734; depth 49.44 2024-11-06T05:00:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869226.01 epoch sec; seqNo:3734; 4061466.16 north; 594826.04 east; 36.69 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T05:00:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869222.81 2024-11-06T05:00:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:26Z- waiting for 1730869226.81, time is 1730869226.01 2024-11-06T05:00:26Z- AHRS_M2 msg: 1730869226.40 epoch sec; seqNo:3508 2024-11-06T05:00:26Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.35 psi 2024-11-06T05:00:26Z- handleDepth()- Depth_Keller msg: 1730869226.41 epoch sec; seqNo:3735; depth 49.45 2024-11-06T05:00:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869226.42 epoch sec; seqNo:3735; 4061465.79 north; 594826.05 east; 36.69 lat; -121.94 lon; 49.45 depth; utmZone: 10 2024-11-06T05:00:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869222.81 2024-11-06T05:00:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:26Z- waiting for 1730869226.81, time is 1730869226.42 2024-11-06T05:00:26Z- AHRS_M2 msg: 1730869226.82 epoch sec; seqNo:3509 2024-11-06T05:00:26Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.35 psi 2024-11-06T05:00:26Z- handleDepth()- Depth_Keller msg: 1730869226.82 epoch sec; seqNo:3736; depth 49.44 2024-11-06T05:00:26Z- handleDVL() - DVL msg epochMillisec: 1730869226.85 2024-11-06T05:00:26Z- handleDvl() - RDI_Pathfinder msg: 1730869226.85 sec; seqNo:1316; #beams:4 2024-11-06T05:00:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.12 , 37.92 , 42.61 , 41.39 2024-11-06T05:00:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.05 , 0.10 2024-11-06T05:00:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869226.86 epoch sec; seqNo:3736; 4061465.41 north; 594826.05 east; 36.69 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T05:00:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869222.81 2024-11-06T05:00:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:26Z- waiting for 1730869226.81, time is 1730869226.86 2024-11-06T05:00:26Z- LcmTrn::updateTrn() >>>> pose time 1730869226.86, meas time 1730869226.85 <<<< 2024-11-06T05:00:26Z- LcmTrn::updateTrn() equating pose 1730869226.85 and meas times 1730869226.85 2024-11-06T05:00:26Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869226.85 2024-11-06T05:00:26Z- TerrainNav::motionUpdate() - currEstimate = (4061465, 594826.05, 49.44) 2024-11-06T05:00:26Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:00:26Z- TerrainNav:: SubcloudNIS is 0.65, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:00:26Z- TNavPF::Projecting Measurements 2024-11-06T05:00:26Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:00:26Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:00:26Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:00:26Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:00:26Z- TNavFilter::4 good beams. 2024-11-06T05:00:26Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:00:26Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:00:26Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:00:26Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:00:26Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:00:26Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:00:26Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:00:26Z- TerrainMapOctree: (vehN, vehE ) = (4061465.41, 594826.05). 2024-11-06T05:00:26Z- TerrainMapOctree: Min Distance = 228.44. 2024-11-06T05:00:26Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:00:27Z- Forcing Subcloud Comparison 2024-11-06T05:00:27Z- Weighting particles with subcloud comparison 2024-11-06T05:00:27Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:27Z- meanExpectedMeasDiff: 0.872916 delta_rms_squared: 1.755359 alpha: 0.988735 2024-11-06T05:00:27Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:27Z- meanExpectedMeasDiff: 0.149652 delta_rms_squared: 0.681384 alpha: 0.971485 2024-11-06T05:00:27Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:27Z- meanExpectedMeasDiff: 1.135791 delta_rms_squared: 0.882020 alpha: 0.977828 2024-11-06T05:00:27Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:27Z- meanExpectedMeasDiff: -1.324553 delta_rms_squared: 0.826070 alpha: 0.976361 2024-11-06T05:00:27Z- Subcloud NIS: 0.593748 2024-11-06T05:00:27Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:00:27Z- TNavPF:: sumWeights = 0.487821 2024-11-06T05:00:27Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:00:27Z- TNavPF::First Term of Map Covariance Matrix is 0.62 2024-11-06T05:00:27Z- TNavPF::Calculated NIS Value : 0.25 numBeams normalized NIS: 0.06 2024-11-06T05:00:27Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869226.85 sec, ping # 0. 2024-11-06T05:00:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:00:27Z- variances([0], [2], and psi_berg) = 0.815241 0.384944 0.000000 2024-11-06T05:00:27Z- publishEstimatesDecoder() - at 1730869226.85 sec 2024-11-06T05:00:27Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:00:27Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061408.75 594831.72 58.64 2024-11-06T05:00:27Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061408.63 594830.56 58.35 lat/lon: 36.69 -121.94 2024-11-06T05:00:27Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.82 0.38 0.35 0.00 2024-11-06T05:00:27Z- AHRS_M2 msg: 1730869227.21 epoch sec; seqNo:3510 2024-11-06T05:00:27Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.35 psi 2024-11-06T05:00:27Z- handleDepth()- Depth_Keller msg: 1730869227.22 epoch sec; seqNo:3737; depth 49.47 2024-11-06T05:00:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869227.25 epoch sec; seqNo:3737; 4061465.03 north; 594826.06 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T05:00:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869226.85 2024-11-06T05:00:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:27Z- waiting for 1730869230.85, time is 1730869227.25 2024-11-06T05:00:27Z- handleDepth()- Depth_Keller msg: 1730869227.64 epoch sec; seqNo:3738; depth 49.47 2024-11-06T05:00:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869227.66 epoch sec; seqNo:3738; 4061464.65 north; 594826.06 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T05:00:27Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:00:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869226.85 2024-11-06T05:00:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:27Z- waiting for 1730869230.85, time is 1730869227.66 2024-11-06T05:00:28Z- AHRS_M2 msg: 1730869228.02 epoch sec; seqNo:3511 2024-11-06T05:00:28Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.35 psi 2024-11-06T05:00:28Z- handleDepth()- Depth_Keller msg: 1730869228.04 epoch sec; seqNo:3739; depth 49.48 2024-11-06T05:00:28Z- handleDVL() - DVL msg epochMillisec: 1730869228.04 2024-11-06T05:00:28Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:28Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:28Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:28Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:00:28Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:00:28Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:00:28Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:00:28Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:00:28Z- handleDvl() - RDI_Pathfinder msg: 1730869228.04 sec; seqNo:1317; #beams:0 2024-11-06T05:00:28Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:00:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.05 , 0.10 2024-11-06T05:00:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869228.05 epoch sec; seqNo:3739; 4061464.65 north; 594826.06 east; 36.69 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T05:00:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869226.85 2024-11-06T05:00:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:28Z- waiting for 1730869230.85, time is 1730869228.05 2024-11-06T05:00:28Z- AHRS_M2 msg: 1730869228.42 epoch sec; seqNo:3512 2024-11-06T05:00:28Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.35 psi 2024-11-06T05:00:28Z- handleDepth()- Depth_Keller msg: 1730869228.43 epoch sec; seqNo:3740; depth 49.49 2024-11-06T05:00:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869228.45 epoch sec; seqNo:3740; 4061464.27 north; 594826.07 east; 36.69 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T05:00:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869226.85 2024-11-06T05:00:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:28Z- waiting for 1730869230.85, time is 1730869228.45 2024-11-06T05:00:28Z- AHRS_M2 msg: 1730869228.84 epoch sec; seqNo:3513 2024-11-06T05:00:28Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.36 psi 2024-11-06T05:00:28Z- handleDepth()- Depth_Keller msg: 1730869228.84 epoch sec; seqNo:3741; depth 49.48 2024-11-06T05:00:28Z- handleDVL() - DVL msg epochMillisec: 1730869228.87 2024-11-06T05:00:28Z- handleDvl() - RDI_Pathfinder msg: 1730869228.87 sec; seqNo:1318; #beams:4 2024-11-06T05:00:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.12 , 37.92 , 42.61 , 41.39 2024-11-06T05:00:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , 0.05 2024-11-06T05:00:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869228.88 epoch sec; seqNo:3741; 4061463.89 north; 594826.07 east; 36.69 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T05:00:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869226.85 2024-11-06T05:00:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:28Z- waiting for 1730869230.85, time is 1730869228.88 2024-11-06T05:00:29Z- AHRS_M2 msg: 1730869229.23 epoch sec; seqNo:3514 2024-11-06T05:00:29Z- AHRS_M2 msg: 0.16 phi; -0.01 theta; 3.36 psi 2024-11-06T05:00:29Z- handleDepth()- Depth_Keller msg: 1730869229.24 epoch sec; seqNo:3742; depth 49.51 2024-11-06T05:00:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869229.26 epoch sec; seqNo:3742; 4061463.50 north; 594824.85 east; 36.69 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T05:00:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869226.85 2024-11-06T05:00:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:29Z- waiting for 1730869230.85, time is 1730869229.26 2024-11-06T05:00:29Z- AHRS_M2 msg: 1730869229.64 epoch sec; seqNo:3515 2024-11-06T05:00:29Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 3.36 psi 2024-11-06T05:00:29Z- handleDepth()- Depth_Keller msg: 1730869229.64 epoch sec; seqNo:3743; depth 49.51 2024-11-06T05:00:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869229.67 epoch sec; seqNo:3743; 4061463.12 north; 594824.86 east; 36.69 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T05:00:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869226.85 2024-11-06T05:00:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:29Z- waiting for 1730869230.85, time is 1730869229.67 2024-11-06T05:00:30Z- AHRS_M2 msg: 1730869230.05 epoch sec; seqNo:3516 2024-11-06T05:00:30Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.36 psi 2024-11-06T05:00:30Z- handleDepth()- Depth_Keller msg: 1730869230.05 epoch sec; seqNo:3744; depth 49.51 2024-11-06T05:00:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869230.07 epoch sec; seqNo:3744; 4061462.74 north; 594824.86 east; 36.69 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T05:00:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869226.85 2024-11-06T05:00:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:30Z- waiting for 1730869230.85, time is 1730869230.07 2024-11-06T05:00:30Z- AHRS_M2 msg: 1730869230.45 epoch sec; seqNo:3517 2024-11-06T05:00:30Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.36 psi 2024-11-06T05:00:30Z- handleDepth()- Depth_Keller msg: 1730869230.46 epoch sec; seqNo:3745; depth 49.52 2024-11-06T05:00:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869230.47 epoch sec; seqNo:3745; 4061462.36 north; 594824.87 east; 36.69 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T05:00:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869226.85 2024-11-06T05:00:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:30Z- waiting for 1730869230.85, time is 1730869230.47 2024-11-06T05:00:30Z- AHRS_M2 msg: 1730869230.85 epoch sec; seqNo:3518 2024-11-06T05:00:30Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.36 psi 2024-11-06T05:00:30Z- handleDepth()- Depth_Keller msg: 1730869230.86 epoch sec; seqNo:3746; depth 49.52 2024-11-06T05:00:30Z- handleDVL() - DVL msg epochMillisec: 1730869230.89 2024-11-06T05:00:30Z- handleDvl() - RDI_Pathfinder msg: 1730869230.89 sec; seqNo:1319; #beams:4 2024-11-06T05:00:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.91 , 38.13 , 42.61 , 41.59 2024-11-06T05:00:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.03 , -0.02 2024-11-06T05:00:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869230.90 epoch sec; seqNo:3746; 4061461.98 north; 594824.87 east; 36.69 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T05:00:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869226.85 2024-11-06T05:00:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:31Z- waiting for 1730869230.85, time is 1730869230.90 2024-11-06T05:00:31Z- LcmTrn::updateTrn() >>>> pose time 1730869230.90, meas time 1730869230.89 <<<< 2024-11-06T05:00:31Z- LcmTrn::updateTrn() equating pose 1730869230.89 and meas times 1730869230.89 2024-11-06T05:00:31Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869230.89 2024-11-06T05:00:31Z- TerrainNav::motionUpdate() - currEstimate = (4061462, 594824.87, 49.52) 2024-11-06T05:00:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:00:31Z- TerrainNav:: SubcloudNIS is 0.59, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:00:31Z- TNavPF::Projecting Measurements 2024-11-06T05:00:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:00:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:00:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:00:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:00:31Z- TNavFilter::4 good beams. 2024-11-06T05:00:31Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:00:31Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:00:31Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:00:31Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:00:31Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:00:31Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:00:31Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:00:31Z- TerrainMapOctree: (vehN, vehE ) = (4061461.98, 594824.87). 2024-11-06T05:00:31Z- TerrainMapOctree: Min Distance = 224.82. 2024-11-06T05:00:31Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:00:31Z- Forcing Subcloud Comparison 2024-11-06T05:00:31Z- Weighting particles with subcloud comparison 2024-11-06T05:00:31Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:31Z- meanExpectedMeasDiff: 1.173651 delta_rms_squared: 1.224914 alpha: 0.983935 2024-11-06T05:00:31Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:31Z- meanExpectedMeasDiff: 0.248275 delta_rms_squared: 0.681364 alpha: 0.971484 2024-11-06T05:00:31Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:31Z- meanExpectedMeasDiff: 1.748569 delta_rms_squared: 0.795708 alpha: 0.975481 2024-11-06T05:00:31Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:31Z- meanExpectedMeasDiff: -1.326012 delta_rms_squared: 0.829404 alpha: 0.976454 2024-11-06T05:00:31Z- Subcloud NIS: 0.551570 2024-11-06T05:00:31Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:00:31Z- TNavPF:: sumWeights = 0.410108 2024-11-06T05:00:31Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:00:31Z- TNavPF::First Term of Map Covariance Matrix is 0.44 2024-11-06T05:00:31Z- TNavPF::Calculated NIS Value : 0.25 numBeams normalized NIS: 0.06 2024-11-06T05:00:31Z- TerrainNav::Resampling particle filter... 2024-11-06T05:00:31Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869230.89 sec, ping # 0. 2024-11-06T05:00:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:00:31Z- variances([0], [2], and psi_berg) = 0.821258 0.385927 0.000000 2024-11-06T05:00:31Z- publishEstimatesDecoder() - at 1730869230.89 sec 2024-11-06T05:00:31Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:00:31Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061407.42 594828.41 58.94 2024-11-06T05:00:31Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061405.21 594829.36 58.27 lat/lon: 36.69 -121.94 2024-11-06T05:00:31Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.82 0.39 0.35 0.00 2024-11-06T05:00:31Z- AHRS_M2 msg: 1730869231.25 epoch sec; seqNo:3519 2024-11-06T05:00:31Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.36 psi 2024-11-06T05:00:31Z- handleDepth()- Depth_Keller msg: 1730869231.26 epoch sec; seqNo:3747; depth 49.51 2024-11-06T05:00:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869231.28 epoch sec; seqNo:3747; 4061461.60 north; 594824.87 east; 36.69 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T05:00:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869230.89 2024-11-06T05:00:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:31Z- waiting for 1730869234.89, time is 1730869231.28 2024-11-06T05:00:31Z- AHRS_M2 msg: 1730869231.66 epoch sec; seqNo:3520 2024-11-06T05:00:31Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.36 psi 2024-11-06T05:00:31Z- handleDepth()- Depth_Keller msg: 1730869231.66 epoch sec; seqNo:3748; depth 49.50 2024-11-06T05:00:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869231.67 epoch sec; seqNo:3748; 4061461.23 north; 594824.88 east; 36.69 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T05:00:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869230.89 2024-11-06T05:00:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:31Z- waiting for 1730869234.89, time is 1730869231.67 2024-11-06T05:00:32Z- AHRS_M2 msg: 1730869232.06 epoch sec; seqNo:3521 2024-11-06T05:00:32Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.36 psi 2024-11-06T05:00:32Z- handleDepth()- Depth_Keller msg: 1730869232.07 epoch sec; seqNo:3749; depth 49.50 2024-11-06T05:00:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869232.07 epoch sec; seqNo:3749; 4061460.85 north; 594824.88 east; 36.69 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T05:00:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869230.89 2024-11-06T05:00:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:32Z- waiting for 1730869234.89, time is 1730869232.07 2024-11-06T05:00:32Z- AHRS_M2 msg: 1730869232.46 epoch sec; seqNo:3522 2024-11-06T05:00:32Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 3.36 psi 2024-11-06T05:00:32Z- handleDepth()- Depth_Keller msg: 1730869232.48 epoch sec; seqNo:3750; depth 49.51 2024-11-06T05:00:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869232.49 epoch sec; seqNo:3750; 4061460.47 north; 594824.89 east; 36.69 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T05:00:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869230.89 2024-11-06T05:00:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:32Z- waiting for 1730869234.89, time is 1730869232.49 2024-11-06T05:00:32Z- AHRS_M2 msg: 1730869232.87 epoch sec; seqNo:3523 2024-11-06T05:00:32Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.36 psi 2024-11-06T05:00:32Z- handleDepth()- Depth_Keller msg: 1730869232.88 epoch sec; seqNo:3751; depth 49.50 2024-11-06T05:00:32Z- handleDVL() - DVL msg epochMillisec: 1730869232.92 2024-11-06T05:00:32Z- handleDvl() - RDI_Pathfinder msg: 1730869232.92 sec; seqNo:1320; #beams:4 2024-11-06T05:00:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.32 , 37.92 , 42.41 , 41.80 2024-11-06T05:00:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , -0.04 2024-11-06T05:00:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869232.93 epoch sec; seqNo:3751; 4061460.09 north; 594824.89 east; 36.69 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T05:00:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869230.89 2024-11-06T05:00:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:33Z- waiting for 1730869234.89, time is 1730869232.93 2024-11-06T05:00:33Z- AHRS_M2 msg: 1730869233.27 epoch sec; seqNo:3524 2024-11-06T05:00:33Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.36 psi 2024-11-06T05:00:33Z- handleDepth()- Depth_Keller msg: 1730869233.28 epoch sec; seqNo:3752; depth 49.49 2024-11-06T05:00:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869233.29 epoch sec; seqNo:3752; 4061459.71 north; 594824.90 east; 36.69 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T05:00:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869230.89 2024-11-06T05:00:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:33Z- waiting for 1730869234.89, time is 1730869233.29 2024-11-06T05:00:33Z- AHRS_M2 msg: 1730869233.68 epoch sec; seqNo:3525 2024-11-06T05:00:33Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.36 psi 2024-11-06T05:00:33Z- handleDepth()- Depth_Keller msg: 1730869233.68 epoch sec; seqNo:3753; depth 49.49 2024-11-06T05:00:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869233.69 epoch sec; seqNo:3753; 4061459.33 north; 594824.90 east; 36.69 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T05:00:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869230.89 2024-11-06T05:00:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:33Z- waiting for 1730869234.89, time is 1730869233.69 2024-11-06T05:00:34Z- AHRS_M2 msg: 1730869234.08 epoch sec; seqNo:3526 2024-11-06T05:00:34Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.36 psi 2024-11-06T05:00:34Z- handleDepth()- Depth_Keller msg: 1730869234.08 epoch sec; seqNo:3754; depth 49.49 2024-11-06T05:00:34Z- handleDVL() - DVL msg epochMillisec: 1730869234.09 2024-11-06T05:00:34Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:34Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:34Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:34Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:00:34Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:00:34Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:00:34Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:00:34Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:00:34Z- handleDvl() - RDI_Pathfinder msg: 1730869234.09 sec; seqNo:1321; #beams:0 2024-11-06T05:00:34Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:00:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.03 , -0.04 2024-11-06T05:00:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869234.11 epoch sec; seqNo:3754; 4061458.95 north; 594824.90 east; 36.69 lat; -121.94 lon; 49.49 depth; utmZone: 10 2024-11-06T05:00:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869230.89 2024-11-06T05:00:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:34Z- waiting for 1730869234.89, time is 1730869234.11 2024-11-06T05:00:34Z- AHRS_M2 msg: 1730869234.48 epoch sec; seqNo:3527 2024-11-06T05:00:34Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.36 psi 2024-11-06T05:00:34Z- handleDepth()- Depth_Keller msg: 1730869234.49 epoch sec; seqNo:3755; depth 49.47 2024-11-06T05:00:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869234.50 epoch sec; seqNo:3755; 4061458.57 north; 594824.91 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T05:00:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869230.89 2024-11-06T05:00:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:34Z- waiting for 1730869234.89, time is 1730869234.50 2024-11-06T05:00:34Z- AHRS_M2 msg: 1730869234.89 epoch sec; seqNo:3528 2024-11-06T05:00:34Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.36 psi 2024-11-06T05:00:34Z- handleDepth()- Depth_Keller msg: 1730869234.90 epoch sec; seqNo:3756; depth 49.47 2024-11-06T05:00:34Z- handleDVL() - DVL msg epochMillisec: 1730869234.92 2024-11-06T05:00:34Z- handleDvl() - RDI_Pathfinder msg: 1730869234.92 sec; seqNo:1322; #beams:4 2024-11-06T05:00:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.91 , 37.92 , 42.41 , 41.59 2024-11-06T05:00:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.09 , -0.02 2024-11-06T05:00:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869234.96 epoch sec; seqNo:3756; 4061458.20 north; 594824.91 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T05:00:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869230.89 2024-11-06T05:00:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:35Z- waiting for 1730869234.89, time is 1730869234.96 2024-11-06T05:00:35Z- LcmTrn::updateTrn() >>>> pose time 1730869234.96, meas time 1730869234.92 <<<< 2024-11-06T05:00:35Z- LcmTrn::updateTrn() equating pose 1730869234.92 and meas times 1730869234.92 2024-11-06T05:00:35Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869234.92 2024-11-06T05:00:35Z- TerrainNav::motionUpdate() - currEstimate = (4061458, 594824.91, 49.47) 2024-11-06T05:00:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:00:35Z- TerrainNav:: SubcloudNIS is 0.55, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:00:35Z- TNavPF::Projecting Measurements 2024-11-06T05:00:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:00:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:00:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:00:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:00:35Z- TNavFilter::4 good beams. 2024-11-06T05:00:35Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:00:35Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:00:35Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:00:35Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:00:35Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:00:35Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:00:35Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:00:35Z- TerrainMapOctree: (vehN, vehE ) = (4061458.20, 594824.91). 2024-11-06T05:00:35Z- TerrainMapOctree: Min Distance = 221.26. 2024-11-06T05:00:35Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:00:35Z- Forcing Subcloud Comparison 2024-11-06T05:00:35Z- Weighting particles with subcloud comparison 2024-11-06T05:00:35Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:35Z- meanExpectedMeasDiff: 0.543430 delta_rms_squared: 1.685335 alpha: 0.988272 2024-11-06T05:00:35Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:35Z- meanExpectedMeasDiff: -0.124837 delta_rms_squared: 0.675994 alpha: 0.971264 2024-11-06T05:00:35Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:35Z- meanExpectedMeasDiff: 1.428384 delta_rms_squared: 0.785786 alpha: 0.975180 2024-11-06T05:00:35Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:35Z- meanExpectedMeasDiff: -1.721759 delta_rms_squared: 0.831043 alpha: 0.976499 2024-11-06T05:00:35Z- Subcloud NIS: 0.506961 2024-11-06T05:00:35Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:00:35Z- TNavPF:: sumWeights = 0.414389 2024-11-06T05:00:35Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:00:35Z- TNavPF::First Term of Map Covariance Matrix is 0.49 2024-11-06T05:00:35Z- TNavPF::Calculated NIS Value : 0.25 numBeams normalized NIS: 0.06 2024-11-06T05:00:35Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869234.92 sec, ping # 0. 2024-11-06T05:00:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:00:35Z- variances([0], [2], and psi_berg) = 0.828380 0.397098 0.000000 2024-11-06T05:00:35Z- publishEstimatesDecoder() - at 1730869234.92 sec 2024-11-06T05:00:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:00:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061400.87 594830.74 58.38 2024-11-06T05:00:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061401.41 594829.41 58.22 lat/lon: 36.69 -121.94 2024-11-06T05:00:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.83 0.40 0.34 0.00 2024-11-06T05:00:35Z- AHRS_M2 msg: 1730869235.29 epoch sec; seqNo:3529 2024-11-06T05:00:35Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.36 psi 2024-11-06T05:00:35Z- handleDepth()- Depth_Keller msg: 1730869235.30 epoch sec; seqNo:3757; depth 49.48 2024-11-06T05:00:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869235.32 epoch sec; seqNo:3757; 4061457.80 north; 594823.70 east; 36.69 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T05:00:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869234.92 2024-11-06T05:00:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:35Z- waiting for 1730869238.92, time is 1730869235.32 2024-11-06T05:00:35Z- AHRS_M2 msg: 1730869235.70 epoch sec; seqNo:3530 2024-11-06T05:00:35Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.36 psi 2024-11-06T05:00:35Z- handleDepth()- Depth_Keller msg: 1730869235.71 epoch sec; seqNo:3758; depth 49.47 2024-11-06T05:00:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869235.72 epoch sec; seqNo:3758; 4061457.42 north; 594823.70 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T05:00:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869234.92 2024-11-06T05:00:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:35Z- waiting for 1730869238.92, time is 1730869235.72 2024-11-06T05:00:36Z- AHRS_M2 msg: 1730869236.10 epoch sec; seqNo:3531 2024-11-06T05:00:36Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 3.36 psi 2024-11-06T05:00:36Z- handleDepth()- Depth_Keller msg: 1730869236.11 epoch sec; seqNo:3759; depth 49.47 2024-11-06T05:00:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869236.12 epoch sec; seqNo:3759; 4061457.42 north; 594823.70 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T05:00:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869234.92 2024-11-06T05:00:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:36Z- waiting for 1730869238.92, time is 1730869236.12 2024-11-06T05:00:36Z- AHRS_M2 msg: 1730869236.50 epoch sec; seqNo:3532 2024-11-06T05:00:36Z- AHRS_M2 msg: 0.17 phi; -0.02 theta; 3.36 psi 2024-11-06T05:00:36Z- handleDepth()- Depth_Keller msg: 1730869236.51 epoch sec; seqNo:3760; depth 49.47 2024-11-06T05:00:36Z- handleDVL() - DVL msg epochMillisec: 1730869236.52 2024-11-06T05:00:36Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:36Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:36Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:36Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:00:36Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:00:36Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:00:36Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:00:36Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:00:36Z- handleDvl() - RDI_Pathfinder msg: 1730869236.52 sec; seqNo:1323; #beams:0 2024-11-06T05:00:36Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:00:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , 0.09 , -0.02 2024-11-06T05:00:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869236.54 epoch sec; seqNo:3760; 4061457.05 north; 594823.70 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T05:00:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869234.92 2024-11-06T05:00:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:36Z- waiting for 1730869238.92, time is 1730869236.54 2024-11-06T05:00:36Z- AHRS_M2 msg: 1730869236.91 epoch sec; seqNo:3533 2024-11-06T05:00:36Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 3.36 psi 2024-11-06T05:00:36Z- handleDepth()- Depth_Keller msg: 1730869236.91 epoch sec; seqNo:3761; depth 49.46 2024-11-06T05:00:36Z- handleDVL() - DVL msg epochMillisec: 1730869236.94 2024-11-06T05:00:36Z- handleDvl() - RDI_Pathfinder msg: 1730869236.94 sec; seqNo:1324; #beams:4 2024-11-06T05:00:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.91 , 37.72 , 42.00 , 41.80 2024-11-06T05:00:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.11 , 0.02 2024-11-06T05:00:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869236.95 epoch sec; seqNo:3761; 4061456.67 north; 594823.71 east; 36.69 lat; -121.94 lon; 49.46 depth; utmZone: 10 2024-11-06T05:00:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869234.92 2024-11-06T05:00:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:37Z- waiting for 1730869238.92, time is 1730869236.95 2024-11-06T05:00:37Z- AHRS_M2 msg: 1730869237.31 epoch sec; seqNo:3534 2024-11-06T05:00:37Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 3.36 psi 2024-11-06T05:00:37Z- handleDepth()- Depth_Keller msg: 1730869237.33 epoch sec; seqNo:3762; depth 49.47 2024-11-06T05:00:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869237.34 epoch sec; seqNo:3762; 4061456.29 north; 594823.71 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T05:00:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869234.92 2024-11-06T05:00:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:37Z- waiting for 1730869238.92, time is 1730869237.34 2024-11-06T05:00:37Z- AHRS_M2 msg: 1730869237.71 epoch sec; seqNo:3535 2024-11-06T05:00:37Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 3.36 psi 2024-11-06T05:00:37Z- handleDepth()- Depth_Keller msg: 1730869237.72 epoch sec; seqNo:3763; depth 49.47 2024-11-06T05:00:37Z- handleDVL() - DVL msg epochMillisec: 1730869237.72 2024-11-06T05:00:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:00:37Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:00:37Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:00:37Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:00:37Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:00:37Z- handleDvl() - RDI_Pathfinder msg: 1730869237.72 sec; seqNo:1325; #beams:0 2024-11-06T05:00:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:00:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , 0.11 , 0.02 2024-11-06T05:00:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869237.73 epoch sec; seqNo:3763; 4061455.91 north; 594823.72 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T05:00:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869234.92 2024-11-06T05:00:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:37Z- waiting for 1730869238.92, time is 1730869237.73 2024-11-06T05:00:38Z- AHRS_M2 msg: 1730869238.12 epoch sec; seqNo:3536 2024-11-06T05:00:38Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 3.36 psi 2024-11-06T05:00:38Z- handleDepth()- Depth_Keller msg: 1730869238.12 epoch sec; seqNo:3764; depth 49.47 2024-11-06T05:00:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869238.13 epoch sec; seqNo:3764; 4061455.53 north; 594823.72 east; 36.69 lat; -121.94 lon; 49.47 depth; utmZone: 10 2024-11-06T05:00:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869234.92 2024-11-06T05:00:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:38Z- waiting for 1730869238.92, time is 1730869238.13 2024-11-06T05:00:38Z- AHRS_M2 msg: 1730869238.52 epoch sec; seqNo:3537 2024-11-06T05:00:38Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 3.36 psi 2024-11-06T05:00:38Z- handleDepth()- Depth_Keller msg: 1730869238.53 epoch sec; seqNo:3765; depth 49.50 2024-11-06T05:00:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869238.55 epoch sec; seqNo:3765; 4061455.15 north; 594823.73 east; 36.69 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T05:00:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869234.92 2024-11-06T05:00:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:38Z- waiting for 1730869238.92, time is 1730869238.55 2024-11-06T05:00:38Z- AHRS_M2 msg: 1730869238.93 epoch sec; seqNo:3538 2024-11-06T05:00:38Z- AHRS_M2 msg: 0.17 phi; -0.03 theta; 3.36 psi 2024-11-06T05:00:38Z- handleDepth()- Depth_Keller msg: 1730869238.93 epoch sec; seqNo:3766; depth 49.50 2024-11-06T05:00:38Z- handleDVL() - DVL msg epochMillisec: 1730869238.97 2024-11-06T05:00:38Z- handleDvl() - RDI_Pathfinder msg: 1730869238.97 sec; seqNo:1326; #beams:4 2024-11-06T05:00:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.91 , 37.72 , 42.21 , 42.00 2024-11-06T05:00:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.10 , 0.05 2024-11-06T05:00:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869238.98 epoch sec; seqNo:3766; 4061454.77 north; 594823.73 east; 36.69 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T05:00:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869234.92 2024-11-06T05:00:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:39Z- waiting for 1730869238.92, time is 1730869238.98 2024-11-06T05:00:39Z- LcmTrn::updateTrn() >>>> pose time 1730869238.98, meas time 1730869238.97 <<<< 2024-11-06T05:00:39Z- LcmTrn::updateTrn() equating pose 1730869238.97 and meas times 1730869238.97 2024-11-06T05:00:39Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869238.97 2024-11-06T05:00:39Z- TerrainNav::motionUpdate() - currEstimate = (4061455, 594823.73, 49.50) 2024-11-06T05:00:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:00:39Z- TerrainNav:: SubcloudNIS is 0.51, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:00:39Z- TNavPF::Projecting Measurements 2024-11-06T05:00:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:00:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:00:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:00:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:00:39Z- TNavFilter::4 good beams. 2024-11-06T05:00:39Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:00:39Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:00:39Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:00:39Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:00:39Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:00:39Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:00:39Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:00:39Z- TerrainMapOctree: (vehN, vehE ) = (4061454.77, 594823.73). 2024-11-06T05:00:39Z- TerrainMapOctree: Min Distance = 217.64. 2024-11-06T05:00:39Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:00:39Z- Forcing Subcloud Comparison 2024-11-06T05:00:39Z- Weighting particles with subcloud comparison 2024-11-06T05:00:39Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:39Z- meanExpectedMeasDiff: 0.527374 delta_rms_squared: 0.960541 alpha: 0.979603 2024-11-06T05:00:39Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:39Z- meanExpectedMeasDiff: -0.182311 delta_rms_squared: 0.646350 alpha: 0.969986 2024-11-06T05:00:39Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:39Z- meanExpectedMeasDiff: 1.284933 delta_rms_squared: 0.755067 alpha: 0.974196 2024-11-06T05:00:39Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:39Z- meanExpectedMeasDiff: -1.483770 delta_rms_squared: 0.807290 alpha: 0.975825 2024-11-06T05:00:39Z- Subcloud NIS: 0.467375 2024-11-06T05:00:39Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:00:39Z- TNavPF:: sumWeights = 0.497529 2024-11-06T05:00:39Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:00:39Z- TNavPF::First Term of Map Covariance Matrix is 0.32 2024-11-06T05:00:39Z- TNavPF::Calculated NIS Value : 0.30 numBeams normalized NIS: 0.08 2024-11-06T05:00:39Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869238.97 sec, ping # 0. 2024-11-06T05:00:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:00:39Z- variances([0], [2], and psi_berg) = 0.826158 0.395911 0.000000 2024-11-06T05:00:39Z- publishEstimatesDecoder() - at 1730869238.97 sec 2024-11-06T05:00:39Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:00:39Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061397.67 594828.53 58.27 2024-11-06T05:00:39Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061397.99 594828.23 58.23 lat/lon: 36.69 -121.94 2024-11-06T05:00:39Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.83 0.40 0.35 0.00 2024-11-06T05:00:39Z- AHRS_M2 msg: 1730869239.33 epoch sec; seqNo:3539 2024-11-06T05:00:39Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.36 psi 2024-11-06T05:00:39Z- handleDepth()- Depth_Keller msg: 1730869239.34 epoch sec; seqNo:3767; depth 49.50 2024-11-06T05:00:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869239.35 epoch sec; seqNo:3767; 4061454.38 north; 594822.51 east; 36.69 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T05:00:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869238.97 2024-11-06T05:00:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:39Z- waiting for 1730869242.97, time is 1730869239.35 2024-11-06T05:00:39Z- AHRS_M2 msg: 1730869239.74 epoch sec; seqNo:3540 2024-11-06T05:00:39Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.36 psi 2024-11-06T05:00:39Z- handleDepth()- Depth_Keller msg: 1730869239.75 epoch sec; seqNo:3768; depth 49.50 2024-11-06T05:00:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869239.76 epoch sec; seqNo:3768; 4061454.00 north; 594822.52 east; 36.69 lat; -121.94 lon; 49.50 depth; utmZone: 10 2024-11-06T05:00:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869238.97 2024-11-06T05:00:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:39Z- waiting for 1730869242.97, time is 1730869239.76 2024-11-06T05:00:40Z- AHRS_M2 msg: 1730869240.14 epoch sec; seqNo:3541 2024-11-06T05:00:40Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.36 psi 2024-11-06T05:00:40Z- handleDepth()- Depth_Keller msg: 1730869240.14 epoch sec; seqNo:3769; depth 49.51 2024-11-06T05:00:40Z- handleDVL() - DVL msg epochMillisec: 1730869240.15 2024-11-06T05:00:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:00:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:00:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:00:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:00:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:00:40Z- handleDvl() - RDI_Pathfinder msg: 1730869240.15 sec; seqNo:1327; #beams:0 2024-11-06T05:00:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:00:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.10 , 0.05 2024-11-06T05:00:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869240.17 epoch sec; seqNo:3769; 4061453.62 north; 594822.52 east; 36.69 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T05:00:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869238.97 2024-11-06T05:00:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:40Z- waiting for 1730869242.97, time is 1730869240.17 2024-11-06T05:00:40Z- AHRS_M2 msg: 1730869240.55 epoch sec; seqNo:3542 2024-11-06T05:00:40Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.36 psi 2024-11-06T05:00:40Z- handleDepth()- Depth_Keller msg: 1730869240.55 epoch sec; seqNo:3770; depth 49.54 2024-11-06T05:00:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869240.56 epoch sec; seqNo:3770; 4061453.24 north; 594822.53 east; 36.69 lat; -121.94 lon; 49.54 depth; utmZone: 10 2024-11-06T05:00:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869238.97 2024-11-06T05:00:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:40Z- waiting for 1730869242.97, time is 1730869240.56 2024-11-06T05:00:40Z- AHRS_M2 msg: 1730869240.95 epoch sec; seqNo:3543 2024-11-06T05:00:40Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.37 psi 2024-11-06T05:00:40Z- handleDepth()- Depth_Keller msg: 1730869240.96 epoch sec; seqNo:3771; depth 49.55 2024-11-06T05:00:40Z- handleDVL() - DVL msg epochMillisec: 1730869240.98 2024-11-06T05:00:40Z- handleDvl() - RDI_Pathfinder msg: 1730869240.98 sec; seqNo:1328; #beams:4 2024-11-06T05:00:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.51 , 37.92 , 42.00 , 42.21 2024-11-06T05:00:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.06 , 0.05 2024-11-06T05:00:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869241.00 epoch sec; seqNo:3771; 4061452.86 north; 594822.53 east; 36.69 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T05:00:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869238.97 2024-11-06T05:00:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:41Z- waiting for 1730869242.97, time is 1730869241.00 2024-11-06T05:00:41Z- AHRS_M2 msg: 1730869241.35 epoch sec; seqNo:3544 2024-11-06T05:00:41Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.37 psi 2024-11-06T05:00:41Z- handleDepth()- Depth_Keller msg: 1730869241.36 epoch sec; seqNo:3772; depth 49.55 2024-11-06T05:00:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869241.38 epoch sec; seqNo:3772; 4061452.49 north; 594822.53 east; 36.69 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T05:00:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869238.97 2024-11-06T05:00:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:41Z- waiting for 1730869242.97, time is 1730869241.38 2024-11-06T05:00:41Z- AHRS_M2 msg: 1730869241.76 epoch sec; seqNo:3545 2024-11-06T05:00:41Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.37 psi 2024-11-06T05:00:41Z- handleDepth()- Depth_Keller msg: 1730869241.77 epoch sec; seqNo:3773; depth 49.57 2024-11-06T05:00:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869241.83 epoch sec; seqNo:3773; 4061452.11 north; 594822.54 east; 36.69 lat; -121.94 lon; 49.57 depth; utmZone: 10 2024-11-06T05:00:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869238.97 2024-11-06T05:00:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:41Z- waiting for 1730869242.97, time is 1730869241.83 2024-11-06T05:00:42Z- AHRS_M2 msg: 1730869242.16 epoch sec; seqNo:3546 2024-11-06T05:00:42Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 3.37 psi 2024-11-06T05:00:42Z- handleDepth()- Depth_Keller msg: 1730869242.16 epoch sec; seqNo:3774; depth 49.58 2024-11-06T05:00:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869242.17 epoch sec; seqNo:3774; 4061452.11 north; 594822.54 east; 36.69 lat; -121.94 lon; 49.58 depth; utmZone: 10 2024-11-06T05:00:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869238.97 2024-11-06T05:00:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:42Z- waiting for 1730869242.97, time is 1730869242.17 2024-11-06T05:00:42Z- AHRS_M2 msg: 1730869242.56 epoch sec; seqNo:3547 2024-11-06T05:00:42Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.37 psi 2024-11-06T05:00:42Z- handleDepth()- Depth_Keller msg: 1730869242.58 epoch sec; seqNo:3775; depth 49.59 2024-11-06T05:00:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869242.59 epoch sec; seqNo:3775; 4061451.73 north; 594822.54 east; 36.69 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T05:00:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869238.97 2024-11-06T05:00:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:42Z- waiting for 1730869242.97, time is 1730869242.59 2024-11-06T05:00:42Z- AHRS_M2 msg: 1730869242.97 epoch sec; seqNo:3548 2024-11-06T05:00:42Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.37 psi 2024-11-06T05:00:42Z- handleDepth()- Depth_Keller msg: 1730869242.97 epoch sec; seqNo:3776; depth 49.60 2024-11-06T05:00:43Z- handleDVL() - DVL msg epochMillisec: 1730869243.00 2024-11-06T05:00:43Z- handleDvl() - RDI_Pathfinder msg: 1730869243.00 sec; seqNo:1329; #beams:4 2024-11-06T05:00:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 37.92 , 42.00 , 42.41 2024-11-06T05:00:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , 0.04 2024-11-06T05:00:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869243.01 epoch sec; seqNo:3776; 4061451.35 north; 594822.55 east; 36.69 lat; -121.94 lon; 49.60 depth; utmZone: 10 2024-11-06T05:00:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869238.97 2024-11-06T05:00:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:43Z- waiting for 1730869242.97, time is 1730869243.01 2024-11-06T05:00:43Z- LcmTrn::updateTrn() >>>> pose time 1730869243.01, meas time 1730869243.00 <<<< 2024-11-06T05:00:43Z- LcmTrn::updateTrn() equating pose 1730869243.00 and meas times 1730869243.00 2024-11-06T05:00:43Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869243.00 2024-11-06T05:00:43Z- TerrainNav::motionUpdate() - currEstimate = (4061451, 594822.55, 49.60) 2024-11-06T05:00:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:00:43Z- TerrainNav:: SubcloudNIS is 0.47, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:00:43Z- TNavPF::Projecting Measurements 2024-11-06T05:00:43Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:00:43Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:00:43Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:00:43Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:00:43Z- TNavFilter::4 good beams. 2024-11-06T05:00:43Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:00:43Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:00:43Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:00:43Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:00:43Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:00:43Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:00:43Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:00:43Z- TerrainMapOctree: (vehN, vehE ) = (4061451.35, 594822.55). 2024-11-06T05:00:43Z- TerrainMapOctree: Min Distance = 214.02. 2024-11-06T05:00:43Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:00:43Z- Forcing Subcloud Comparison 2024-11-06T05:00:43Z- Weighting particles with subcloud comparison 2024-11-06T05:00:43Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:43Z- meanExpectedMeasDiff: 0.649776 delta_rms_squared: 0.970604 alpha: 0.979810 2024-11-06T05:00:43Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:43Z- meanExpectedMeasDiff: 0.035868 delta_rms_squared: 0.662011 alpha: 0.970675 2024-11-06T05:00:43Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:43Z- meanExpectedMeasDiff: 1.388616 delta_rms_squared: 0.762165 alpha: 0.974430 2024-11-06T05:00:43Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:43Z- meanExpectedMeasDiff: -1.122628 delta_rms_squared: 0.829062 alpha: 0.976445 2024-11-06T05:00:43Z- Subcloud NIS: 0.428566 2024-11-06T05:00:43Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:00:43Z- TNavPF:: sumWeights = 0.532829 2024-11-06T05:00:43Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:00:43Z- TNavPF::First Term of Map Covariance Matrix is 0.34 2024-11-06T05:00:43Z- TNavPF::Calculated NIS Value : 0.30 numBeams normalized NIS: 0.07 2024-11-06T05:00:43Z- TerrainNav::Resampling particle filter... 2024-11-06T05:00:43Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869243.00 sec, ping # 0. 2024-11-06T05:00:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:00:43Z- variances([0], [2], and psi_berg) = 0.826078 0.396210 0.000000 2024-11-06T05:00:43Z- publishEstimatesDecoder() - at 1730869243.00 sec 2024-11-06T05:00:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:00:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061396.73 594826.06 59.44 2024-11-06T05:00:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061394.56 594827.04 58.26 lat/lon: 36.69 -121.94 2024-11-06T05:00:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.83 0.40 0.34 0.00 2024-11-06T05:00:43Z- AHRS_M2 msg: 1730869243.37 epoch sec; seqNo:3549 2024-11-06T05:00:43Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.37 psi 2024-11-06T05:00:43Z- handleDepth()- Depth_Keller msg: 1730869243.38 epoch sec; seqNo:3777; depth 49.61 2024-11-06T05:00:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869243.39 epoch sec; seqNo:3777; 4061450.97 north; 594822.55 east; 36.69 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T05:00:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869243.00 2024-11-06T05:00:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:43Z- waiting for 1730869247.00, time is 1730869243.39 2024-11-06T05:00:43Z- AHRS_M2 msg: 1730869243.77 epoch sec; seqNo:3550 2024-11-06T05:00:43Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.37 psi 2024-11-06T05:00:43Z- handleDepth()- Depth_Keller msg: 1730869243.79 epoch sec; seqNo:3778; depth 49.62 2024-11-06T05:00:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869243.81 epoch sec; seqNo:3778; 4061450.59 north; 594822.56 east; 36.69 lat; -121.94 lon; 49.62 depth; utmZone: 10 2024-11-06T05:00:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869243.00 2024-11-06T05:00:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:43Z- waiting for 1730869247.00, time is 1730869243.81 2024-11-06T05:00:44Z- AHRS_M2 msg: 1730869244.18 epoch sec; seqNo:3551 2024-11-06T05:00:44Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.37 psi 2024-11-06T05:00:44Z- handleDepth()- Depth_Keller msg: 1730869244.18 epoch sec; seqNo:3779; depth 49.61 2024-11-06T05:00:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869244.19 epoch sec; seqNo:3779; 4061450.20 north; 594821.34 east; 36.69 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T05:00:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869243.00 2024-11-06T05:00:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:44Z- waiting for 1730869247.00, time is 1730869244.19 2024-11-06T05:00:44Z- AHRS_M2 msg: 1730869244.58 epoch sec; seqNo:3552 2024-11-06T05:00:44Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.37 psi 2024-11-06T05:00:44Z- handleDepth()- Depth_Keller msg: 1730869244.60 epoch sec; seqNo:3780; depth 49.63 2024-11-06T05:00:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869244.60 epoch sec; seqNo:3780; 4061449.82 north; 594821.34 east; 36.69 lat; -121.94 lon; 49.63 depth; utmZone: 10 2024-11-06T05:00:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869243.00 2024-11-06T05:00:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:44Z- waiting for 1730869247.00, time is 1730869244.60 2024-11-06T05:00:45Z- AHRS_M2 msg: 1730869244.99 epoch sec; seqNo:3553 2024-11-06T05:00:45Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.37 psi 2024-11-06T05:00:45Z- handleDepth()- Depth_Keller msg: 1730869245.00 epoch sec; seqNo:3781; depth 49.64 2024-11-06T05:00:45Z- handleDVL() - DVL msg epochMillisec: 1730869245.02 2024-11-06T05:00:45Z- handleDvl() - RDI_Pathfinder msg: 1730869245.02 sec; seqNo:1330; #beams:4 2024-11-06T05:00:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.91 , 37.92 , 42.21 , 42.41 2024-11-06T05:00:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.03 2024-11-06T05:00:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869245.04 epoch sec; seqNo:3781; 4061449.44 north; 594821.35 east; 36.69 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T05:00:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869243.00 2024-11-06T05:00:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:45Z- waiting for 1730869247.00, time is 1730869245.04 2024-11-06T05:00:45Z- AHRS_M2 msg: 1730869245.39 epoch sec; seqNo:3554 2024-11-06T05:00:45Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.38 psi 2024-11-06T05:00:45Z- handleDepth()- Depth_Keller msg: 1730869245.39 epoch sec; seqNo:3782; depth 49.64 2024-11-06T05:00:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869245.42 epoch sec; seqNo:3782; 4061449.06 north; 594821.35 east; 36.69 lat; -121.94 lon; 49.64 depth; utmZone: 10 2024-11-06T05:00:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869243.00 2024-11-06T05:00:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:45Z- waiting for 1730869247.00, time is 1730869245.42 2024-11-06T05:00:45Z- AHRS_M2 msg: 1730869245.80 epoch sec; seqNo:3555 2024-11-06T05:00:45Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.38 psi 2024-11-06T05:00:45Z- handleDepth()- Depth_Keller msg: 1730869245.80 epoch sec; seqNo:3783; depth 49.65 2024-11-06T05:00:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869245.81 epoch sec; seqNo:3783; 4061448.68 north; 594821.36 east; 36.69 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T05:00:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869243.00 2024-11-06T05:00:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:45Z- waiting for 1730869247.00, time is 1730869245.81 2024-11-06T05:00:46Z- AHRS_M2 msg: 1730869246.20 epoch sec; seqNo:3556 2024-11-06T05:00:46Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.38 psi 2024-11-06T05:00:46Z- handleDepth()- Depth_Keller msg: 1730869246.20 epoch sec; seqNo:3784; depth 49.66 2024-11-06T05:00:46Z- handleDVL() - DVL msg epochMillisec: 1730869246.21 2024-11-06T05:00:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:00:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:00:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:00:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:00:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:00:46Z- handleDvl() - RDI_Pathfinder msg: 1730869246.21 sec; seqNo:1331; #beams:0 2024-11-06T05:00:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:00:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.01 , 0.03 2024-11-06T05:00:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869246.22 epoch sec; seqNo:3784; 4061448.30 north; 594821.36 east; 36.69 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T05:00:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869243.00 2024-11-06T05:00:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:46Z- waiting for 1730869247.00, time is 1730869246.22 2024-11-06T05:00:46Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T05:00:46Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T05:00:46Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T05:00:46Z- AHRS_M2 msg: 1730869246.60 epoch sec; seqNo:3557 2024-11-06T05:00:46Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.38 psi 2024-11-06T05:00:46Z- handleDepth()- Depth_Keller msg: 1730869246.63 epoch sec; seqNo:3785; depth 49.66 2024-11-06T05:00:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869246.64 epoch sec; seqNo:3785; 4061447.93 north; 594821.36 east; 36.69 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T05:00:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869243.00 2024-11-06T05:00:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:46Z- waiting for 1730869247.00, time is 1730869246.64 2024-11-06T05:00:47Z- AHRS_M2 msg: 1730869247.01 epoch sec; seqNo:3558 2024-11-06T05:00:47Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.38 psi 2024-11-06T05:00:47Z- handleDepth()- Depth_Keller msg: 1730869247.02 epoch sec; seqNo:3786; depth 49.66 2024-11-06T05:00:47Z- handleDVL() - DVL msg epochMillisec: 1730869247.05 2024-11-06T05:00:47Z- handleDvl() - RDI_Pathfinder msg: 1730869247.05 sec; seqNo:1332; #beams:4 2024-11-06T05:00:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.51 , 37.92 , 42.00 , 42.41 2024-11-06T05:00:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.03 , 0.01 2024-11-06T05:00:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869247.06 epoch sec; seqNo:3786; 4061447.55 north; 594821.37 east; 36.69 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T05:00:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869243.00 2024-11-06T05:00:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:47Z- waiting for 1730869247.00, time is 1730869247.06 2024-11-06T05:00:47Z- LcmTrn::updateTrn() >>>> pose time 1730869247.06, meas time 1730869247.05 <<<< 2024-11-06T05:00:47Z- LcmTrn::updateTrn() equating pose 1730869247.05 and meas times 1730869247.05 2024-11-06T05:00:47Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869247.05 2024-11-06T05:00:47Z- TerrainNav::motionUpdate() - currEstimate = (4061448, 594821.37, 49.66) 2024-11-06T05:00:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:00:47Z- TerrainNav:: SubcloudNIS is 0.43, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:00:47Z- TNavPF::Projecting Measurements 2024-11-06T05:00:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:00:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:00:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:00:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:00:47Z- TNavFilter::4 good beams. 2024-11-06T05:00:47Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:00:47Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:00:47Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:00:47Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:00:47Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:00:47Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:00:47Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:00:47Z- TerrainMapOctree: (vehN, vehE ) = (4061447.55, 594821.37). 2024-11-06T05:00:47Z- TerrainMapOctree: Min Distance = 210.04. 2024-11-06T05:00:47Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:00:47Z- Forcing Subcloud Comparison 2024-11-06T05:00:47Z- Weighting particles with subcloud comparison 2024-11-06T05:00:47Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:47Z- meanExpectedMeasDiff: 0.422443 delta_rms_squared: 0.984861 alpha: 0.980097 2024-11-06T05:00:47Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:47Z- meanExpectedMeasDiff: 0.029705 delta_rms_squared: 0.671294 alpha: 0.971069 2024-11-06T05:00:47Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:47Z- meanExpectedMeasDiff: 1.466596 delta_rms_squared: 0.769574 alpha: 0.974670 2024-11-06T05:00:47Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:47Z- meanExpectedMeasDiff: -1.238946 delta_rms_squared: 0.844932 alpha: 0.976877 2024-11-06T05:00:47Z- Subcloud NIS: 0.385775 2024-11-06T05:00:47Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:00:47Z- TNavPF:: sumWeights = 0.512022 2024-11-06T05:00:47Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:00:47Z- TNavPF::First Term of Map Covariance Matrix is 0.32 2024-11-06T05:00:47Z- TNavPF::Calculated NIS Value : 0.30 numBeams normalized NIS: 0.07 2024-11-06T05:00:47Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869247.05 sec, ping # 0. 2024-11-06T05:00:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:00:47Z- variances([0], [2], and psi_berg) = 0.829770 0.394936 0.000000 2024-11-06T05:00:47Z- publishEstimatesDecoder() - at 1730869247.05 sec 2024-11-06T05:00:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:00:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061391.94 594825.78 58.20 2024-11-06T05:00:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061390.77 594825.86 58.28 lat/lon: 36.69 -121.94 2024-11-06T05:00:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.83 0.39 0.35 0.00 2024-11-06T05:00:47Z- AHRS_M2 msg: 1730869247.42 epoch sec; seqNo:3559 2024-11-06T05:00:47Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.39 psi 2024-11-06T05:00:47Z- handleDepth()- Depth_Keller msg: 1730869247.44 epoch sec; seqNo:3787; depth 49.67 2024-11-06T05:00:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869247.45 epoch sec; seqNo:3787; 4061447.17 north; 594821.37 east; 36.69 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T05:00:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869247.05 2024-11-06T05:00:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:47Z- waiting for 1730869251.05, time is 1730869247.45 2024-11-06T05:00:47Z- AHRS_M2 msg: 1730869247.83 epoch sec; seqNo:3560 2024-11-06T05:00:47Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.39 psi 2024-11-06T05:00:47Z- handleDepth()- Depth_Keller msg: 1730869247.83 epoch sec; seqNo:3788; depth 49.67 2024-11-06T05:00:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869247.85 epoch sec; seqNo:3788; 4061446.79 north; 594821.38 east; 36.69 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T05:00:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869247.05 2024-11-06T05:00:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:47Z- waiting for 1730869251.05, time is 1730869247.85 2024-11-06T05:00:48Z- AHRS_M2 msg: 1730869248.22 epoch sec; seqNo:3561 2024-11-06T05:00:48Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.39 psi 2024-11-06T05:00:48Z- handleDepth()- Depth_Keller msg: 1730869248.23 epoch sec; seqNo:3789; depth 49.68 2024-11-06T05:00:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869248.24 epoch sec; seqNo:3789; 4061446.41 north; 594821.38 east; 36.69 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T05:00:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869247.05 2024-11-06T05:00:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:48Z- waiting for 1730869251.05, time is 1730869248.24 2024-11-06T05:00:48Z- AHRS_M2 msg: 1730869248.63 epoch sec; seqNo:3562 2024-11-06T05:00:48Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.39 psi 2024-11-06T05:00:48Z- handleDepth()- Depth_Keller msg: 1730869248.63 epoch sec; seqNo:3790; depth 49.67 2024-11-06T05:00:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869248.64 epoch sec; seqNo:3790; 4061446.03 north; 594821.39 east; 36.69 lat; -121.94 lon; 49.67 depth; utmZone: 10 2024-11-06T05:00:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869247.05 2024-11-06T05:00:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:48Z- waiting for 1730869251.05, time is 1730869248.64 2024-11-06T05:00:49Z- AHRS_M2 msg: 1730869249.03 epoch sec; seqNo:3563 2024-11-06T05:00:49Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.39 psi 2024-11-06T05:00:49Z- handleDepth()- Depth_Keller msg: 1730869249.03 epoch sec; seqNo:3791; depth 49.69 2024-11-06T05:00:49Z- handleDVL() - DVL msg epochMillisec: 1730869249.06 2024-11-06T05:00:49Z- handleDvl() - RDI_Pathfinder msg: 1730869249.06 sec; seqNo:1333; #beams:4 2024-11-06T05:00:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 37.92 , 41.80 , 42.41 2024-11-06T05:00:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.05 , -0.01 2024-11-06T05:00:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869249.08 epoch sec; seqNo:3791; 4061445.64 north; 594820.17 east; 36.69 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T05:00:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869247.05 2024-11-06T05:00:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:49Z- waiting for 1730869251.05, time is 1730869249.08 2024-11-06T05:00:49Z- AHRS_M2 msg: 1730869249.43 epoch sec; seqNo:3564 2024-11-06T05:00:49Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.39 psi 2024-11-06T05:00:49Z- handleDepth()- Depth_Keller msg: 1730869249.44 epoch sec; seqNo:3792; depth 49.68 2024-11-06T05:00:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869249.45 epoch sec; seqNo:3792; 4061445.64 north; 594820.17 east; 36.69 lat; -121.94 lon; 49.68 depth; utmZone: 10 2024-11-06T05:00:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869247.05 2024-11-06T05:00:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:49Z- waiting for 1730869251.05, time is 1730869249.45 2024-11-06T05:00:49Z- AHRS_M2 msg: 1730869249.84 epoch sec; seqNo:3565 2024-11-06T05:00:49Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.39 psi 2024-11-06T05:00:49Z- handleDepth()- Depth_Keller msg: 1730869249.85 epoch sec; seqNo:3793; depth 49.70 2024-11-06T05:00:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869249.86 epoch sec; seqNo:3793; 4061445.26 north; 594820.17 east; 36.69 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T05:00:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869247.05 2024-11-06T05:00:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:50Z- waiting for 1730869251.05, time is 1730869249.86 2024-11-06T05:00:50Z- AHRS_M2 msg: 1730869250.24 epoch sec; seqNo:3566 2024-11-06T05:00:50Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.39 psi 2024-11-06T05:00:50Z- handleDepth()- Depth_Keller msg: 1730869250.25 epoch sec; seqNo:3794; depth 49.69 2024-11-06T05:00:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869250.26 epoch sec; seqNo:3794; 4061444.88 north; 594820.18 east; 36.69 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T05:00:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869247.05 2024-11-06T05:00:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:50Z- waiting for 1730869251.05, time is 1730869250.26 2024-11-06T05:00:50Z- AHRS_M2 msg: 1730869250.64 epoch sec; seqNo:3567 2024-11-06T05:00:50Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.39 psi 2024-11-06T05:00:50Z- handleDepth()- Depth_Keller msg: 1730869250.65 epoch sec; seqNo:3795; depth 49.69 2024-11-06T05:00:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869250.67 epoch sec; seqNo:3795; 4061444.50 north; 594820.18 east; 36.69 lat; -121.94 lon; 49.69 depth; utmZone: 10 2024-11-06T05:00:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869247.05 2024-11-06T05:00:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:50Z- waiting for 1730869251.05, time is 1730869250.67 2024-11-06T05:00:51Z- AHRS_M2 msg: 1730869251.05 epoch sec; seqNo:3568 2024-11-06T05:00:51Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.39 psi 2024-11-06T05:00:51Z- handleDepth()- Depth_Keller msg: 1730869251.05 epoch sec; seqNo:3796; depth 49.70 2024-11-06T05:00:51Z- handleDVL() - DVL msg epochMillisec: 1730869251.07 2024-11-06T05:00:51Z- handleDvl() - RDI_Pathfinder msg: 1730869251.07 sec; seqNo:1334; #beams:4 2024-11-06T05:00:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.71 , 37.92 , 41.80 , 42.41 2024-11-06T05:00:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.09 , 0.01 2024-11-06T05:00:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869251.08 epoch sec; seqNo:3796; 4061444.12 north; 594820.19 east; 36.69 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T05:00:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869247.05 2024-11-06T05:00:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:51Z- waiting for 1730869251.05, time is 1730869251.08 2024-11-06T05:00:51Z- LcmTrn::updateTrn() >>>> pose time 1730869251.08, meas time 1730869251.07 <<<< 2024-11-06T05:00:51Z- LcmTrn::updateTrn() equating pose 1730869251.07 and meas times 1730869251.07 2024-11-06T05:00:51Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869251.07 2024-11-06T05:00:51Z- TerrainNav::motionUpdate() - currEstimate = (4061444, 594820.19, 49.70) 2024-11-06T05:00:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:00:51Z- TerrainNav:: SubcloudNIS is 0.39, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:00:51Z- TNavPF::Projecting Measurements 2024-11-06T05:00:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:00:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:00:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:00:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:00:51Z- TNavFilter::4 good beams. 2024-11-06T05:00:51Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:00:51Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:00:51Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:00:51Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:00:51Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:00:51Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:00:51Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:00:51Z- TerrainMapOctree: (vehN, vehE ) = (4061444.12, 594820.19). 2024-11-06T05:00:51Z- TerrainMapOctree: Min Distance = 206.42. 2024-11-06T05:00:51Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:00:51Z- Forcing Subcloud Comparison 2024-11-06T05:00:51Z- Weighting particles with subcloud comparison 2024-11-06T05:00:51Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:51Z- meanExpectedMeasDiff: 0.577529 delta_rms_squared: 0.992091 alpha: 0.980239 2024-11-06T05:00:51Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:51Z- meanExpectedMeasDiff: 0.013223 delta_rms_squared: 0.675425 alpha: 0.971241 2024-11-06T05:00:51Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:51Z- meanExpectedMeasDiff: 1.263706 delta_rms_squared: 0.773576 alpha: 0.974798 2024-11-06T05:00:51Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:51Z- meanExpectedMeasDiff: -1.302629 delta_rms_squared: 0.851905 alpha: 0.977062 2024-11-06T05:00:51Z- Subcloud NIS: 0.341138 2024-11-06T05:00:51Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:00:51Z- TNavPF:: sumWeights = 0.524464 2024-11-06T05:00:51Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:00:51Z- TNavPF::First Term of Map Covariance Matrix is 0.34 2024-11-06T05:00:51Z- TNavPF::Calculated NIS Value : 0.30 numBeams normalized NIS: 0.07 2024-11-06T05:00:51Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869251.07 sec, ping # 0. 2024-11-06T05:00:51Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:00:51Z- variances([0], [2], and psi_berg) = 0.833147 0.394222 0.000000 2024-11-06T05:00:51Z- publishEstimatesDecoder() - at 1730869251.07 sec 2024-11-06T05:00:51Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:00:51Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061387.27 594824.60 58.21 2024-11-06T05:00:51Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061387.34 594824.67 58.27 lat/lon: 36.69 -121.94 2024-11-06T05:00:51Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.83 0.39 0.35 0.00 2024-11-06T05:00:51Z- AHRS_M2 msg: 1730869251.45 epoch sec; seqNo:3569 2024-11-06T05:00:51Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.39 psi 2024-11-06T05:00:51Z- handleDepth()- Depth_Keller msg: 1730869251.46 epoch sec; seqNo:3797; depth 49.70 2024-11-06T05:00:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869251.50 epoch sec; seqNo:3797; 4061443.75 north; 594820.19 east; 36.69 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T05:00:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869251.07 2024-11-06T05:00:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:51Z- waiting for 1730869255.07, time is 1730869251.50 2024-11-06T05:00:51Z- AHRS_M2 msg: 1730869251.86 epoch sec; seqNo:3570 2024-11-06T05:00:51Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.39 psi 2024-11-06T05:00:51Z- handleDepth()- Depth_Keller msg: 1730869251.87 epoch sec; seqNo:3798; depth 49.71 2024-11-06T05:00:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869251.89 epoch sec; seqNo:3798; 4061443.37 north; 594820.19 east; 36.69 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T05:00:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869251.07 2024-11-06T05:00:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:52Z- waiting for 1730869255.07, time is 1730869251.89 2024-11-06T05:00:52Z- AHRS_M2 msg: 1730869252.26 epoch sec; seqNo:3571 2024-11-06T05:00:52Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.39 psi 2024-11-06T05:00:52Z- handleDepth()- Depth_Keller msg: 1730869252.27 epoch sec; seqNo:3799; depth 49.71 2024-11-06T05:00:52Z- handleDVL() - DVL msg epochMillisec: 1730869252.27 2024-11-06T05:00:52Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:52Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:52Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:52Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:00:52Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:00:52Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:00:52Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:00:52Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:00:52Z- handleDvl() - RDI_Pathfinder msg: 1730869252.27 sec; seqNo:1335; #beams:0 2024-11-06T05:00:52Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:00:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.09 , 0.01 2024-11-06T05:00:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869252.28 epoch sec; seqNo:3799; 4061442.99 north; 594820.20 east; 36.69 lat; -121.94 lon; 49.71 depth; utmZone: 10 2024-11-06T05:00:52Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869251.07 2024-11-06T05:00:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:52Z- waiting for 1730869255.07, time is 1730869252.28 2024-11-06T05:00:52Z- AHRS_M2 msg: 1730869252.67 epoch sec; seqNo:3572 2024-11-06T05:00:52Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.39 psi 2024-11-06T05:00:52Z- handleDepth()- Depth_Keller msg: 1730869252.69 epoch sec; seqNo:3800; depth 49.72 2024-11-06T05:00:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869252.75 epoch sec; seqNo:3800; 4061442.61 north; 594820.20 east; 36.69 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T05:00:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869251.07 2024-11-06T05:00:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:52Z- waiting for 1730869255.07, time is 1730869252.75 2024-11-06T05:00:53Z- AHRS_M2 msg: 1730869253.07 epoch sec; seqNo:3573 2024-11-06T05:00:53Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.39 psi 2024-11-06T05:00:53Z- handleDepth()- Depth_Keller msg: 1730869253.07 epoch sec; seqNo:3801; depth 49.72 2024-11-06T05:00:53Z- handleDVL() - DVL msg epochMillisec: 1730869253.09 2024-11-06T05:00:53Z- handleDvl() - RDI_Pathfinder msg: 1730869253.09 sec; seqNo:1336; #beams:4 2024-11-06T05:00:53Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.51 , 38.13 , 41.59 , 42.41 2024-11-06T05:00:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.09 , 0.04 2024-11-06T05:00:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869253.11 epoch sec; seqNo:3801; 4061442.22 north; 594818.99 east; 36.69 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T05:00:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869251.07 2024-11-06T05:00:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:53Z- waiting for 1730869255.07, time is 1730869253.11 2024-11-06T05:00:53Z- AHRS_M2 msg: 1730869253.47 epoch sec; seqNo:3574 2024-11-06T05:00:53Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.39 psi 2024-11-06T05:00:53Z- handleDepth()- Depth_Keller msg: 1730869253.48 epoch sec; seqNo:3802; depth 49.73 2024-11-06T05:00:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869253.50 epoch sec; seqNo:3802; 4061441.84 north; 594818.99 east; 36.69 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T05:00:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869251.07 2024-11-06T05:00:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:53Z- waiting for 1730869255.07, time is 1730869253.50 2024-11-06T05:00:53Z- AHRS_M2 msg: 1730869253.88 epoch sec; seqNo:3575 2024-11-06T05:00:53Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.39 psi 2024-11-06T05:00:53Z- handleDepth()- Depth_Keller msg: 1730869253.88 epoch sec; seqNo:3803; depth 49.75 2024-11-06T05:00:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869253.89 epoch sec; seqNo:3803; 4061441.46 north; 594818.99 east; 36.69 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T05:00:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869251.07 2024-11-06T05:00:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:54Z- waiting for 1730869255.07, time is 1730869253.89 2024-11-06T05:00:54Z- AHRS_M2 msg: 1730869254.28 epoch sec; seqNo:3576 2024-11-06T05:00:54Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.39 psi 2024-11-06T05:00:54Z- handleDepth()- Depth_Keller msg: 1730869254.28 epoch sec; seqNo:3804; depth 49.75 2024-11-06T05:00:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869254.30 epoch sec; seqNo:3804; 4061441.08 north; 594819.00 east; 36.69 lat; -121.94 lon; 49.75 depth; utmZone: 10 2024-11-06T05:00:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869251.07 2024-11-06T05:00:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:54Z- waiting for 1730869255.07, time is 1730869254.30 2024-11-06T05:00:54Z- AHRS_M2 msg: 1730869254.75 epoch sec; seqNo:3577 2024-11-06T05:00:54Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.40 psi 2024-11-06T05:00:54Z- handleDepth()- Depth_Keller msg: 1730869254.75 epoch sec; seqNo:3805; depth 49.78 2024-11-06T05:00:54Z- handleDVL() - DVL msg epochMillisec: 1730869254.78 2024-11-06T05:00:54Z- handleDvl() - RDI_Pathfinder msg: 1730869254.78 sec; seqNo:1337; #beams:4 2024-11-06T05:00:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.51 , 37.92 , 41.59 , 42.61 2024-11-06T05:00:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , 0.06 2024-11-06T05:00:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869254.80 epoch sec; seqNo:3805; 4061440.70 north; 594819.00 east; 36.69 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T05:00:54Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869251.07 2024-11-06T05:00:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:54Z- waiting for 1730869255.07, time is 1730869254.80 2024-11-06T05:00:55Z- AHRS_M2 msg: 1730869255.09 epoch sec; seqNo:3578 2024-11-06T05:00:55Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.40 psi 2024-11-06T05:00:55Z- handleDepth()- Depth_Keller msg: 1730869255.10 epoch sec; seqNo:3806; depth 49.78 2024-11-06T05:00:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869255.11 epoch sec; seqNo:3806; 4061440.70 north; 594819.00 east; 36.69 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T05:00:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869251.07 2024-11-06T05:00:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:55Z- waiting for 1730869255.07, time is 1730869255.11 2024-11-06T05:00:55Z- LcmTrn::updateTrn() >>>> pose time 1730869255.11, meas time 1730869254.78 <<<< 2024-11-06T05:00:55Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869255.11 2024-11-06T05:00:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869254.78 sec, ping # 0; waiting for INS data more recent than 1730869251.07... 2024-11-06T05:00:55Z- TerrainNav::motionUpdate() - currEstimate = (4061441, 594819.00, 49.78) 2024-11-06T05:00:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:00:55Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:00:55Z- TNavPF::Projecting Measurements 2024-11-06T05:00:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:00:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:00:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:00:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:00:55Z- TNavFilter::4 good beams. 2024-11-06T05:00:55Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:00:55Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:00:55Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:00:55Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:00:55Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:00:55Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:00:55Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:00:55Z- TerrainMapOctree: (vehN, vehE ) = (4061440.98, 594819.10). 2024-11-06T05:00:55Z- TerrainMapOctree: Min Distance = 203.10. 2024-11-06T05:00:55Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:00:55Z- Forcing Subcloud Comparison 2024-11-06T05:00:55Z- Weighting particles with subcloud comparison 2024-11-06T05:00:55Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:55Z- meanExpectedMeasDiff: 0.397087 delta_rms_squared: 1.025446 alpha: 0.980869 2024-11-06T05:00:55Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:55Z- meanExpectedMeasDiff: 0.046794 delta_rms_squared: 0.697700 alpha: 0.972133 2024-11-06T05:00:55Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:55Z- meanExpectedMeasDiff: 1.187606 delta_rms_squared: 0.795369 alpha: 0.975471 2024-11-06T05:00:55Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:55Z- meanExpectedMeasDiff: -1.176669 delta_rms_squared: 0.900542 alpha: 0.978274 2024-11-06T05:00:55Z- Subcloud NIS: 0.331538 2024-11-06T05:00:55Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:00:55Z- TNavPF:: sumWeights = 0.556349 2024-11-06T05:00:55Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:00:55Z- TNavPF::First Term of Map Covariance Matrix is 0.35 2024-11-06T05:00:55Z- TNavPF::Calculated NIS Value : 0.28 numBeams normalized NIS: 0.07 2024-11-06T05:00:55Z- TerrainNav::Resampling particle filter... 2024-11-06T05:00:55Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869254.78 sec. 2024-11-06T05:00:55Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:00:55Z- variances([0], [2], and psi_berg) = 0.842781 0.399170 0.000000 2024-11-06T05:00:55Z- publishEstimatesDecoder() - at 1730869255.11 sec 2024-11-06T05:00:55Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:00:55Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061386.16 594822.34 58.50 2024-11-06T05:00:55Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061383.93 594823.49 58.31 lat/lon: 36.69 -121.94 2024-11-06T05:00:55Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.84 0.40 0.60 0.00 2024-11-06T05:00:55Z- AHRS_M2 msg: 1730869255.49 epoch sec; seqNo:3579 2024-11-06T05:00:55Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.40 psi 2024-11-06T05:00:55Z- handleDepth()- Depth_Keller msg: 1730869255.50 epoch sec; seqNo:3807; depth 49.78 2024-11-06T05:00:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869255.53 epoch sec; seqNo:3807; 4061440.32 north; 594819.01 east; 36.69 lat; -121.94 lon; 49.78 depth; utmZone: 10 2024-11-06T05:00:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869255.11 2024-11-06T05:00:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:55Z- waiting for 1730869259.11, time is 1730869255.53 2024-11-06T05:00:55Z- AHRS_M2 msg: 1730869255.89 epoch sec; seqNo:3580 2024-11-06T05:00:55Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.40 psi 2024-11-06T05:00:55Z- handleDepth()- Depth_Keller msg: 1730869255.91 epoch sec; seqNo:3808; depth 49.80 2024-11-06T05:00:55Z- handleDVL() - DVL msg epochMillisec: 1730869255.91 2024-11-06T05:00:55Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:55Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:55Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:55Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:00:55Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:00:55Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:00:55Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:00:55Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:00:55Z- handleDvl() - RDI_Pathfinder msg: 1730869255.91 sec; seqNo:1338; #beams:0 2024-11-06T05:00:55Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:00:55Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , 0.06 2024-11-06T05:00:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869255.92 epoch sec; seqNo:3808; 4061439.94 north; 594819.01 east; 36.69 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T05:00:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869255.11 2024-11-06T05:00:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:56Z- waiting for 1730869259.11, time is 1730869255.92 2024-11-06T05:00:56Z- AHRS_M2 msg: 1730869256.30 epoch sec; seqNo:3581 2024-11-06T05:00:56Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.40 psi 2024-11-06T05:00:56Z- handleDepth()- Depth_Keller msg: 1730869256.33 epoch sec; seqNo:3809; depth 49.80 2024-11-06T05:00:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869256.35 epoch sec; seqNo:3809; 4061439.56 north; 594819.02 east; 36.69 lat; -121.94 lon; 49.80 depth; utmZone: 10 2024-11-06T05:00:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869255.11 2024-11-06T05:00:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:56Z- waiting for 1730869259.11, time is 1730869256.35 2024-11-06T05:00:56Z- AHRS_M2 msg: 1730869256.75 epoch sec; seqNo:3582 2024-11-06T05:00:56Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.41 psi 2024-11-06T05:00:56Z- handleDepth()- Depth_Keller msg: 1730869256.76 epoch sec; seqNo:3810; depth 49.81 2024-11-06T05:00:56Z- handleDVL() - DVL msg epochMillisec: 1730869256.78 2024-11-06T05:00:56Z- handleDvl() - RDI_Pathfinder msg: 1730869256.78 sec; seqNo:1339; #beams:4 2024-11-06T05:00:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.51 , 38.13 , 41.59 , 42.82 2024-11-06T05:00:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.03 , 0.03 2024-11-06T05:00:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869256.79 epoch sec; seqNo:3810; 4061439.19 north; 594819.02 east; 36.69 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T05:00:56Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869255.11 2024-11-06T05:00:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:56Z- waiting for 1730869259.11, time is 1730869256.79 2024-11-06T05:00:57Z- AHRS_M2 msg: 1730869257.11 epoch sec; seqNo:3583 2024-11-06T05:00:57Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.41 psi 2024-11-06T05:00:57Z- handleDepth()- Depth_Keller msg: 1730869257.12 epoch sec; seqNo:3811; depth 49.84 2024-11-06T05:00:57Z- handleDVL() - DVL msg epochMillisec: 1730869257.12 2024-11-06T05:00:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:00:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:00:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:00:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:00:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:00:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:00:57Z- handleDvl() - RDI_Pathfinder msg: 1730869257.12 sec; seqNo:1340; #beams:0 2024-11-06T05:00:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:00:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.03 , 0.03 2024-11-06T05:00:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869257.14 epoch sec; seqNo:3811; 4061438.81 north; 594819.02 east; 36.69 lat; -121.94 lon; 49.84 depth; utmZone: 10 2024-11-06T05:00:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869255.11 2024-11-06T05:00:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:57Z- waiting for 1730869259.11, time is 1730869257.14 2024-11-06T05:00:57Z- AHRS_M2 msg: 1730869257.55 epoch sec; seqNo:3584 2024-11-06T05:00:57Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.41 psi 2024-11-06T05:00:57Z- handleDepth()- Depth_Keller msg: 1730869257.56 epoch sec; seqNo:3812; depth 49.83 2024-11-06T05:00:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869257.58 epoch sec; seqNo:3812; 4061438.41 north; 594817.81 east; 36.69 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T05:00:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869255.11 2024-11-06T05:00:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:57Z- waiting for 1730869259.11, time is 1730869257.58 2024-11-06T05:00:57Z- AHRS_M2 msg: 1730869257.92 epoch sec; seqNo:3585 2024-11-06T05:00:57Z- AHRS_M2 msg: 0.17 phi; -0.05 theta; 3.41 psi 2024-11-06T05:00:57Z- handleDepth()- Depth_Keller msg: 1730869257.92 epoch sec; seqNo:3813; depth 49.85 2024-11-06T05:00:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869257.93 epoch sec; seqNo:3813; 4061438.04 north; 594817.81 east; 36.69 lat; -121.94 lon; 49.85 depth; utmZone: 10 2024-11-06T05:00:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869255.11 2024-11-06T05:00:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:58Z- waiting for 1730869259.11, time is 1730869257.93 2024-11-06T05:00:58Z- AHRS_M2 msg: 1730869258.32 epoch sec; seqNo:3586 2024-11-06T05:00:58Z- AHRS_M2 msg: 0.17 phi; -0.05 theta; 3.41 psi 2024-11-06T05:00:58Z- handleDepth()- Depth_Keller msg: 1730869258.33 epoch sec; seqNo:3814; depth 49.86 2024-11-06T05:00:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869258.35 epoch sec; seqNo:3814; 4061437.66 north; 594817.82 east; 36.69 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T05:00:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869255.11 2024-11-06T05:00:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:58Z- waiting for 1730869259.11, time is 1730869258.35 2024-11-06T05:00:58Z- AHRS_M2 msg: 1730869258.76 epoch sec; seqNo:3587 2024-11-06T05:00:58Z- AHRS_M2 msg: 0.17 phi; -0.05 theta; 3.41 psi 2024-11-06T05:00:58Z- handleDepth()- Depth_Keller msg: 1730869258.77 epoch sec; seqNo:3815; depth 49.88 2024-11-06T05:00:58Z- handleDVL() - DVL msg epochMillisec: 1730869258.79 2024-11-06T05:00:58Z- handleDvl() - RDI_Pathfinder msg: 1730869258.79 sec; seqNo:1341; #beams:4 2024-11-06T05:00:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.51 , 37.92 , 41.19 , 42.82 2024-11-06T05:00:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.06 , 0.01 2024-11-06T05:00:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869258.80 epoch sec; seqNo:3815; 4061437.28 north; 594817.82 east; 36.69 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T05:00:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:00:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869255.11 2024-11-06T05:00:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:58Z- waiting for 1730869259.11, time is 1730869258.80 2024-11-06T05:00:59Z- AHRS_M2 msg: 1730869259.13 epoch sec; seqNo:3588 2024-11-06T05:00:59Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.41 psi 2024-11-06T05:00:59Z- handleDepth()- Depth_Keller msg: 1730869259.13 epoch sec; seqNo:3816; depth 49.88 2024-11-06T05:00:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869259.15 epoch sec; seqNo:3816; 4061436.90 north; 594817.82 east; 36.69 lat; -121.94 lon; 49.88 depth; utmZone: 10 2024-11-06T05:00:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869255.11 2024-11-06T05:00:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:59Z- waiting for 1730869259.11, time is 1730869259.15 2024-11-06T05:00:59Z- LcmTrn::updateTrn() >>>> pose time 1730869259.15, meas time 1730869258.79 <<<< 2024-11-06T05:00:59Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869259.15 2024-11-06T05:00:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869258.79 sec, ping # 0; waiting for INS data more recent than 1730869255.11... 2024-11-06T05:00:59Z- TerrainNav::motionUpdate() - currEstimate = (4061437, 594817.82, 49.88) 2024-11-06T05:00:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:00:59Z- TerrainNav:: SubcloudNIS is 0.33, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:00:59Z- TNavPF::Projecting Measurements 2024-11-06T05:00:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:00:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:00:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:00:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:00:59Z- TNavFilter::4 good beams. 2024-11-06T05:00:59Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:00:59Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:00:59Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:00:59Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:00:59Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:00:59Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:00:59Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:00:59Z- TerrainMapOctree: (vehN, vehE ) = (4061437.24, 594817.93). 2024-11-06T05:00:59Z- TerrainMapOctree: Min Distance = 199.18. 2024-11-06T05:00:59Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:00:59Z- Forcing Subcloud Comparison 2024-11-06T05:00:59Z- Weighting particles with subcloud comparison 2024-11-06T05:00:59Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:59Z- meanExpectedMeasDiff: 0.454075 delta_rms_squared: 1.407099 alpha: 0.985986 2024-11-06T05:00:59Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:59Z- meanExpectedMeasDiff: 0.119538 delta_rms_squared: 0.957274 alpha: 0.979535 2024-11-06T05:00:59Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:59Z- meanExpectedMeasDiff: 0.952047 delta_rms_squared: 1.085596 alpha: 0.981910 2024-11-06T05:00:59Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:00:59Z- meanExpectedMeasDiff: -0.753693 delta_rms_squared: 1.942581 alpha: 0.989809 2024-11-06T05:00:59Z- Subcloud NIS: 0.317150 2024-11-06T05:00:59Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:00:59Z- TNavPF:: sumWeights = 0.562514 2024-11-06T05:00:59Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:00:59Z- TNavPF::First Term of Map Covariance Matrix is 0.40 2024-11-06T05:00:59Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T05:00:59Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869258.79 sec. 2024-11-06T05:00:59Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:00:59Z- variances([0], [2], and psi_berg) = 0.857434 0.410939 0.000000 2024-11-06T05:00:59Z- publishEstimatesDecoder() - at 1730869259.15 sec 2024-11-06T05:00:59Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:00:59Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061379.13 594822.21 56.97 2024-11-06T05:00:59Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061380.11 594822.31 58.36 lat/lon: 36.69 -121.94 2024-11-06T05:00:59Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.86 0.41 0.66 0.00 2024-11-06T05:00:59Z- AHRS_M2 msg: 1730869259.53 epoch sec; seqNo:3589 2024-11-06T05:00:59Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.41 psi 2024-11-06T05:00:59Z- handleDepth()- Depth_Keller msg: 1730869259.54 epoch sec; seqNo:3817; depth 49.89 2024-11-06T05:00:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869259.57 epoch sec; seqNo:3817; 4061436.52 north; 594817.83 east; 36.69 lat; -121.94 lon; 49.89 depth; utmZone: 10 2024-11-06T05:00:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:00:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869259.15 2024-11-06T05:00:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:00:59Z- waiting for 1730869263.15, time is 1730869259.57 2024-11-06T05:00:59Z- AHRS_M2 msg: 1730869259.93 epoch sec; seqNo:3590 2024-11-06T05:00:59Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.41 psi 2024-11-06T05:00:59Z- handleDepth()- Depth_Keller msg: 1730869259.94 epoch sec; seqNo:3818; depth 49.91 2024-11-06T05:00:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869259.95 epoch sec; seqNo:3818; 4061436.14 north; 594817.83 east; 36.69 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T05:01:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869259.15 2024-11-06T05:01:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:00Z- waiting for 1730869263.15, time is 1730869259.95 2024-11-06T05:01:00Z- AHRS_M2 msg: 1730869260.34 epoch sec; seqNo:3591 2024-11-06T05:01:00Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.41 psi 2024-11-06T05:01:00Z- handleDepth()- Depth_Keller msg: 1730869260.36 epoch sec; seqNo:3819; depth 49.91 2024-11-06T05:01:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869260.37 epoch sec; seqNo:3819; 4061435.76 north; 594817.84 east; 36.69 lat; -121.94 lon; 49.91 depth; utmZone: 10 2024-11-06T05:01:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869259.15 2024-11-06T05:01:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:00Z- waiting for 1730869263.15, time is 1730869260.37 2024-11-06T05:01:00Z- AHRS_M2 msg: 1730869260.76 epoch sec; seqNo:3592 2024-11-06T05:01:00Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.41 psi 2024-11-06T05:01:00Z- handleDepth()- Depth_Keller msg: 1730869260.77 epoch sec; seqNo:3820; depth 49.92 2024-11-06T05:01:00Z- handleDVL() - DVL msg epochMillisec: 1730869260.80 2024-11-06T05:01:00Z- handleDvl() - RDI_Pathfinder msg: 1730869260.80 sec; seqNo:1342; #beams:4 2024-11-06T05:01:00Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 37.92 , 40.98 , 42.82 2024-11-06T05:01:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.06 , 0.02 2024-11-06T05:01:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869260.82 epoch sec; seqNo:3820; 4061435.38 north; 594817.84 east; 36.69 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T05:01:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869259.15 2024-11-06T05:01:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:00Z- waiting for 1730869263.15, time is 1730869260.82 2024-11-06T05:01:01Z- AHRS_M2 msg: 1730869261.15 epoch sec; seqNo:3593 2024-11-06T05:01:01Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.42 psi 2024-11-06T05:01:01Z- handleDepth()- Depth_Keller msg: 1730869261.15 epoch sec; seqNo:3821; depth 49.92 2024-11-06T05:01:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869261.17 epoch sec; seqNo:3821; 4061435.00 north; 594817.85 east; 36.69 lat; -121.94 lon; 49.92 depth; utmZone: 10 2024-11-06T05:01:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869259.15 2024-11-06T05:01:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:01Z- waiting for 1730869263.15, time is 1730869261.17 2024-11-06T05:01:01Z- AHRS_M2 msg: 1730869261.55 epoch sec; seqNo:3594 2024-11-06T05:01:01Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.42 psi 2024-11-06T05:01:01Z- handleDepth()- Depth_Keller msg: 1730869261.57 epoch sec; seqNo:3822; depth 49.94 2024-11-06T05:01:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869261.58 epoch sec; seqNo:3822; 4061434.99 north; 594816.63 east; 36.69 lat; -121.94 lon; 49.94 depth; utmZone: 10 2024-11-06T05:01:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869259.15 2024-11-06T05:01:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:01Z- waiting for 1730869263.15, time is 1730869261.58 2024-11-06T05:01:01Z- AHRS_M2 msg: 1730869261.95 epoch sec; seqNo:3595 2024-11-06T05:01:01Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.42 psi 2024-11-06T05:01:01Z- handleDepth()- Depth_Keller msg: 1730869261.96 epoch sec; seqNo:3823; depth 49.96 2024-11-06T05:01:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869261.97 epoch sec; seqNo:3823; 4061434.61 north; 594816.63 east; 36.69 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T05:01:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869259.15 2024-11-06T05:01:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:02Z- waiting for 1730869263.15, time is 1730869261.97 2024-11-06T05:01:02Z- handleDepth()- Depth_Keller msg: 1730869262.38 epoch sec; seqNo:3824; depth 49.96 2024-11-06T05:01:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869262.41 epoch sec; seqNo:3824; 4061434.23 north; 594816.63 east; 36.69 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T05:01:02Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:01:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869259.15 2024-11-06T05:01:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:02Z- waiting for 1730869263.15, time is 1730869262.41 2024-11-06T05:01:02Z- AHRS_M2 msg: 1730869262.77 epoch sec; seqNo:3596 2024-11-06T05:01:02Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.42 psi 2024-11-06T05:01:02Z- handleDepth()- Depth_Keller msg: 1730869262.78 epoch sec; seqNo:3825; depth 49.98 2024-11-06T05:01:02Z- handleDVL() - DVL msg epochMillisec: 1730869262.80 2024-11-06T05:01:02Z- handleDvl() - RDI_Pathfinder msg: 1730869262.80 sec; seqNo:1343; #beams:4 2024-11-06T05:01:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.10 , 38.13 , 40.98 , 42.82 2024-11-06T05:01:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.06 , 0.04 2024-11-06T05:01:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869262.84 epoch sec; seqNo:3825; 4061433.85 north; 594816.64 east; 36.69 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T05:01:02Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869259.15 2024-11-06T05:01:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:02Z- waiting for 1730869263.15, time is 1730869262.84 2024-11-06T05:01:03Z- AHRS_M2 msg: 1730869263.17 epoch sec; seqNo:3597 2024-11-06T05:01:03Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.42 psi 2024-11-06T05:01:03Z- handleDepth()- Depth_Keller msg: 1730869263.18 epoch sec; seqNo:3826; depth 49.98 2024-11-06T05:01:03Z- handleDVL() - DVL msg epochMillisec: 1730869263.18 2024-11-06T05:01:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:01:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:01:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:01:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:01:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:01:03Z- handleDvl() - RDI_Pathfinder msg: 1730869263.18 sec; seqNo:1344; #beams:0 2024-11-06T05:01:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:01:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.06 , 0.04 2024-11-06T05:01:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869263.20 epoch sec; seqNo:3826; 4061433.48 north; 594816.64 east; 36.69 lat; -121.94 lon; 49.98 depth; utmZone: 10 2024-11-06T05:01:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869259.15 2024-11-06T05:01:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:03Z- waiting for 1730869263.15, time is 1730869263.20 2024-11-06T05:01:03Z- LcmTrn::updateTrn() >>>> pose time 1730869263.20, meas time 1730869263.18 <<<< 2024-11-06T05:01:03Z- LcmTrn::updateTrn() equating pose 1730869263.18 and meas times 1730869263.18 2024-11-06T05:01:03Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869263.18 2024-11-06T05:01:03Z- TerrainNav::motionUpdate() - currEstimate = (4061433, 594816.64, 49.98) 2024-11-06T05:01:03Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:01:03Z- TerrainNav:: SubcloudNIS is 0.32, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:01:03Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T05:01:03Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T05:01:03Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T05:01:03Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:01:03Z- variances([0], [2], and psi_berg) = 0.859391 0.413759 0.000000 2024-11-06T05:01:03Z- publishEstimatesDecoder() - at 1730869263.18 sec 2024-11-06T05:01:03Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:01:03Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061375.71 594820.99 56.76 2024-11-06T05:01:03Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061376.69 594821.12 58.45 lat/lon: 36.69 -121.94 2024-11-06T05:01:03Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.86 0.41 0.90 0.00 2024-11-06T05:01:03Z- AHRS_M2 msg: 1730869263.57 epoch sec; seqNo:3598 2024-11-06T05:01:03Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.42 psi 2024-11-06T05:01:03Z- handleDepth()- Depth_Keller msg: 1730869263.58 epoch sec; seqNo:3827; depth 50.00 2024-11-06T05:01:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869263.59 epoch sec; seqNo:3827; 4061433.10 north; 594816.65 east; 36.69 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T05:01:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869263.18 2024-11-06T05:01:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:03Z- waiting for 1730869267.18, time is 1730869263.59 2024-11-06T05:01:03Z- AHRS_M2 msg: 1730869263.97 epoch sec; seqNo:3599 2024-11-06T05:01:03Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.42 psi 2024-11-06T05:01:03Z- handleDepth()- Depth_Keller msg: 1730869263.98 epoch sec; seqNo:3828; depth 50.01 2024-11-06T05:01:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869264.00 epoch sec; seqNo:3828; 4061432.72 north; 594816.65 east; 36.69 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T05:01:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869263.18 2024-11-06T05:01:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:04Z- waiting for 1730869267.18, time is 1730869264.00 2024-11-06T05:01:04Z- AHRS_M2 msg: 1730869264.39 epoch sec; seqNo:3600 2024-11-06T05:01:04Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.43 psi 2024-11-06T05:01:04Z- handleDepth()- Depth_Keller msg: 1730869264.39 epoch sec; seqNo:3829; depth 50.02 2024-11-06T05:01:04Z- handleDVL() - DVL msg epochMillisec: 1730869264.40 2024-11-06T05:01:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:01:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:01:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:01:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:01:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:01:04Z- handleDvl() - RDI_Pathfinder msg: 1730869264.40 sec; seqNo:1345; #beams:0 2024-11-06T05:01:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:01:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.06 , 0.04 2024-11-06T05:01:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869264.41 epoch sec; seqNo:3829; 4061432.34 north; 594816.65 east; 36.69 lat; -121.94 lon; 50.02 depth; utmZone: 10 2024-11-06T05:01:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869263.18 2024-11-06T05:01:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:04Z- waiting for 1730869267.18, time is 1730869264.41 2024-11-06T05:01:04Z- AHRS_M2 msg: 1730869264.79 epoch sec; seqNo:3601 2024-11-06T05:01:04Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.43 psi 2024-11-06T05:01:04Z- handleDepth()- Depth_Keller msg: 1730869264.81 epoch sec; seqNo:3830; depth 50.03 2024-11-06T05:01:04Z- handleDVL() - DVL msg epochMillisec: 1730869264.84 2024-11-06T05:01:04Z- handleDvl() - RDI_Pathfinder msg: 1730869264.84 sec; seqNo:1346; #beams:4 2024-11-06T05:01:04Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.30 , 38.13 , 40.98 , 42.82 2024-11-06T05:01:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.04 , 0.04 2024-11-06T05:01:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869264.86 epoch sec; seqNo:3830; 4061431.96 north; 594816.66 east; 36.69 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T05:01:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869263.18 2024-11-06T05:01:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:04Z- waiting for 1730869267.18, time is 1730869264.86 2024-11-06T05:01:05Z- AHRS_M2 msg: 1730869265.19 epoch sec; seqNo:3602 2024-11-06T05:01:05Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.43 psi 2024-11-06T05:01:05Z- handleDepth()- Depth_Keller msg: 1730869265.19 epoch sec; seqNo:3831; depth 50.03 2024-11-06T05:01:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869265.21 epoch sec; seqNo:3831; 4061431.58 north; 594816.66 east; 36.69 lat; -121.94 lon; 50.03 depth; utmZone: 10 2024-11-06T05:01:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869263.18 2024-11-06T05:01:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:05Z- waiting for 1730869267.18, time is 1730869265.21 2024-11-06T05:01:05Z- AHRS_M2 msg: 1730869265.60 epoch sec; seqNo:3603 2024-11-06T05:01:05Z- AHRS_M2 msg: 0.16 phi; -0.05 theta; 3.43 psi 2024-11-06T05:01:05Z- handleDepth()- Depth_Keller msg: 1730869265.60 epoch sec; seqNo:3832; depth 50.04 2024-11-06T05:01:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869265.61 epoch sec; seqNo:3832; 4061431.19 north; 594815.45 east; 36.69 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T05:01:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869263.18 2024-11-06T05:01:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:05Z- waiting for 1730869267.18, time is 1730869265.61 2024-11-06T05:01:06Z- AHRS_M2 msg: 1730869265.99 epoch sec; seqNo:3604 2024-11-06T05:01:06Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.43 psi 2024-11-06T05:01:06Z- handleDepth()- Depth_Keller msg: 1730869266.00 epoch sec; seqNo:3833; depth 50.04 2024-11-06T05:01:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869266.01 epoch sec; seqNo:3833; 4061430.81 north; 594815.45 east; 36.69 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T05:01:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869263.18 2024-11-06T05:01:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:06Z- waiting for 1730869267.18, time is 1730869266.01 2024-11-06T05:01:06Z- AHRS_M2 msg: 1730869266.40 epoch sec; seqNo:3605 2024-11-06T05:01:06Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.43 psi 2024-11-06T05:01:06Z- handleDepth()- Depth_Keller msg: 1730869266.41 epoch sec; seqNo:3834; depth 50.05 2024-11-06T05:01:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869266.43 epoch sec; seqNo:3834; 4061430.43 north; 594815.46 east; 36.69 lat; -121.94 lon; 50.05 depth; utmZone: 10 2024-11-06T05:01:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869263.18 2024-11-06T05:01:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:06Z- waiting for 1730869267.18, time is 1730869266.43 2024-11-06T05:01:06Z- AHRS_M2 msg: 1730869266.81 epoch sec; seqNo:3606 2024-11-06T05:01:06Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.43 psi 2024-11-06T05:01:06Z- handleDepth()- Depth_Keller msg: 1730869266.82 epoch sec; seqNo:3835; depth 50.06 2024-11-06T05:01:06Z- handleDVL() - DVL msg epochMillisec: 1730869266.85 2024-11-06T05:01:06Z- handleDvl() - RDI_Pathfinder msg: 1730869266.85 sec; seqNo:1347; #beams:4 2024-11-06T05:01:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.69 , 37.92 , 40.78 , 42.82 2024-11-06T05:01:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.03 , 0.01 2024-11-06T05:01:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869266.87 epoch sec; seqNo:3835; 4061430.05 north; 594815.46 east; 36.69 lat; -121.94 lon; 50.06 depth; utmZone: 10 2024-11-06T05:01:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869263.18 2024-11-06T05:01:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:06Z- waiting for 1730869267.18, time is 1730869266.87 2024-11-06T05:01:07Z- handleDepth()- Depth_Keller msg: 1730869267.23 epoch sec; seqNo:3836; depth 50.07 2024-11-06T05:01:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869267.24 epoch sec; seqNo:3836; 4061429.67 north; 594815.46 east; 36.69 lat; -121.94 lon; 50.07 depth; utmZone: 10 2024-11-06T05:01:07Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:01:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869263.18 2024-11-06T05:01:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:07Z- waiting for 1730869267.18, time is 1730869267.24 2024-11-06T05:01:07Z- LcmTrn::updateTrn() >>>> pose time 1730869267.24, meas time 1730869266.85 <<<< 2024-11-06T05:01:07Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869267.24 2024-11-06T05:01:07Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869266.85 sec, ping # 0; waiting for INS data more recent than 1730869263.18... 2024-11-06T05:01:07Z- TerrainNav::motionUpdate() - currEstimate = (4061430, 594815.46, 50.07) 2024-11-06T05:01:07Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:01:07Z- TerrainNav:: SubcloudNIS is 0.32, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:01:07Z- TNavPF::Projecting Measurements 2024-11-06T05:01:07Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:01:07Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:01:07Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:01:07Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:01:07Z- TNavFilter::4 good beams. 2024-11-06T05:01:07Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:01:07Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:01:07Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:01:07Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:01:07Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:01:07Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:01:07Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:01:07Z- TerrainMapOctree: (vehN, vehE ) = (4061430.04, 594815.58). 2024-11-06T05:01:07Z- TerrainMapOctree: Min Distance = 191.61. 2024-11-06T05:01:07Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:01:07Z- Forcing Subcloud Comparison 2024-11-06T05:01:07Z- Weighting particles with subcloud comparison 2024-11-06T05:01:07Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:07Z- meanExpectedMeasDiff: 0.323727 delta_rms_squared: 1.879417 alpha: 0.989470 2024-11-06T05:01:07Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:07Z- meanExpectedMeasDiff: 0.056188 delta_rms_squared: 1.322292 alpha: 0.985100 2024-11-06T05:01:07Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:07Z- meanExpectedMeasDiff: 0.788304 delta_rms_squared: 1.476369 alpha: 0.986634 2024-11-06T05:01:07Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:07Z- meanExpectedMeasDiff: -0.132748 delta_rms_squared: 1.846408 alpha: 0.989284 2024-11-06T05:01:07Z- Subcloud NIS: 0.303592 2024-11-06T05:01:07Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:01:07Z- TNavPF:: sumWeights = 0.595494 2024-11-06T05:01:07Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:01:07Z- TNavPF::First Term of Map Covariance Matrix is 0.48 2024-11-06T05:01:07Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.02 2024-11-06T05:01:07Z- TerrainNav::Resampling particle filter... 2024-11-06T05:01:07Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869266.85 sec. 2024-11-06T05:01:07Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:01:07Z- variances([0], [2], and psi_berg) = 0.849169 0.421057 0.000000 2024-11-06T05:01:07Z- publishEstimatesDecoder() - at 1730869267.24 sec 2024-11-06T05:01:07Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:01:07Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061375.17 594818.85 59.36 2024-11-06T05:01:07Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061372.91 594819.95 58.42 lat/lon: 36.69 -121.94 2024-11-06T05:01:07Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.85 0.42 0.75 0.00 2024-11-06T05:01:07Z- AHRS_M2 msg: 1730869267.62 epoch sec; seqNo:3607 2024-11-06T05:01:07Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.43 psi 2024-11-06T05:01:07Z- handleDepth()- Depth_Keller msg: 1730869267.64 epoch sec; seqNo:3837; depth 50.09 2024-11-06T05:01:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869267.67 epoch sec; seqNo:3837; 4061429.29 north; 594815.47 east; 36.69 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T05:01:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869267.24 2024-11-06T05:01:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:07Z- waiting for 1730869271.24, time is 1730869267.67 2024-11-06T05:01:08Z- AHRS_M2 msg: 1730869268.02 epoch sec; seqNo:3608 2024-11-06T05:01:08Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.43 psi 2024-11-06T05:01:08Z- handleDepth()- Depth_Keller msg: 1730869268.03 epoch sec; seqNo:3838; depth 50.09 2024-11-06T05:01:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869268.04 epoch sec; seqNo:3838; 4061429.29 north; 594815.47 east; 36.69 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T05:01:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869267.24 2024-11-06T05:01:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:08Z- waiting for 1730869271.24, time is 1730869268.04 2024-11-06T05:01:08Z- AHRS_M2 msg: 1730869268.42 epoch sec; seqNo:3609 2024-11-06T05:01:08Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.43 psi 2024-11-06T05:01:08Z- handleDepth()- Depth_Keller msg: 1730869268.42 epoch sec; seqNo:3839; depth 50.08 2024-11-06T05:01:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869268.44 epoch sec; seqNo:3839; 4061428.92 north; 594815.47 east; 36.69 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T05:01:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869267.24 2024-11-06T05:01:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:08Z- waiting for 1730869271.24, time is 1730869268.44 2024-11-06T05:01:08Z- AHRS_M2 msg: 1730869268.83 epoch sec; seqNo:3610 2024-11-06T05:01:08Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.43 psi 2024-11-06T05:01:08Z- handleDepth()- Depth_Keller msg: 1730869268.84 epoch sec; seqNo:3840; depth 50.10 2024-11-06T05:01:08Z- handleDVL() - DVL msg epochMillisec: 1730869268.86 2024-11-06T05:01:08Z- handleDvl() - RDI_Pathfinder msg: 1730869268.86 sec; seqNo:1348; #beams:4 2024-11-06T05:01:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.69 , 38.33 , 40.98 , 43.02 2024-11-06T05:01:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.05 , -0.00 2024-11-06T05:01:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869268.87 epoch sec; seqNo:3840; 4061428.54 north; 594815.48 east; 36.69 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T05:01:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869267.24 2024-11-06T05:01:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:08Z- waiting for 1730869271.24, time is 1730869268.87 2024-11-06T05:01:09Z- AHRS_M2 msg: 1730869269.23 epoch sec; seqNo:3611 2024-11-06T05:01:09Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.43 psi 2024-11-06T05:01:09Z- handleDepth()- Depth_Keller msg: 1730869269.23 epoch sec; seqNo:3841; depth 50.12 2024-11-06T05:01:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869269.25 epoch sec; seqNo:3841; 4061428.16 north; 594815.48 east; 36.69 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T05:01:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869267.24 2024-11-06T05:01:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:09Z- waiting for 1730869271.24, time is 1730869269.25 2024-11-06T05:01:09Z- AHRS_M2 msg: 1730869269.63 epoch sec; seqNo:3612 2024-11-06T05:01:09Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.43 psi 2024-11-06T05:01:09Z- handleDepth()- Depth_Keller msg: 1730869269.65 epoch sec; seqNo:3842; depth 50.12 2024-11-06T05:01:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869269.65 epoch sec; seqNo:3842; 4061427.77 north; 594814.26 east; 36.69 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T05:01:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869267.24 2024-11-06T05:01:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:09Z- waiting for 1730869271.24, time is 1730869269.65 2024-11-06T05:01:10Z- AHRS_M2 msg: 1730869270.04 epoch sec; seqNo:3613 2024-11-06T05:01:10Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.43 psi 2024-11-06T05:01:10Z- handleDepth()- Depth_Keller msg: 1730869270.05 epoch sec; seqNo:3843; depth 50.13 2024-11-06T05:01:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869270.06 epoch sec; seqNo:3843; 4061427.39 north; 594814.27 east; 36.69 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T05:01:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869267.24 2024-11-06T05:01:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:10Z- waiting for 1730869271.24, time is 1730869270.06 2024-11-06T05:01:10Z- AHRS_M2 msg: 1730869270.44 epoch sec; seqNo:3614 2024-11-06T05:01:10Z- AHRS_M2 msg: 0.15 phi; -0.05 theta; 3.43 psi 2024-11-06T05:01:10Z- handleDepth()- Depth_Keller msg: 1730869270.44 epoch sec; seqNo:3844; depth 50.15 2024-11-06T05:01:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869270.45 epoch sec; seqNo:3844; 4061427.01 north; 594814.27 east; 36.69 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T05:01:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869267.24 2024-11-06T05:01:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:10Z- waiting for 1730869271.24, time is 1730869270.45 2024-11-06T05:01:10Z- handleDepth()- Depth_Keller msg: 1730869270.87 epoch sec; seqNo:3845; depth 50.15 2024-11-06T05:01:10Z- handleDVL() - DVL msg epochMillisec: 1730869270.89 2024-11-06T05:01:10Z- handleDvl() - RDI_Pathfinder msg: 1730869270.89 sec; seqNo:1349; #beams:4 2024-11-06T05:01:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.89 , 37.92 , 40.78 , 42.82 2024-11-06T05:01:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.09 , 0.00 2024-11-06T05:01:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869270.91 epoch sec; seqNo:3845; 4061426.63 north; 594814.28 east; 36.69 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T05:01:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869267.24 2024-11-06T05:01:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:11Z- waiting for 1730869271.24, time is 1730869270.91 2024-11-06T05:01:11Z- AHRS_M2 msg: 1730869271.25 epoch sec; seqNo:3615 2024-11-06T05:01:11Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.43 psi 2024-11-06T05:01:11Z- handleDepth()- Depth_Keller msg: 1730869271.26 epoch sec; seqNo:3846; depth 50.15 2024-11-06T05:01:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869271.28 epoch sec; seqNo:3846; 4061426.25 north; 594814.28 east; 36.69 lat; -121.94 lon; 50.15 depth; utmZone: 10 2024-11-06T05:01:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869267.24 2024-11-06T05:01:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:11Z- waiting for 1730869271.24, time is 1730869271.28 2024-11-06T05:01:11Z- LcmTrn::updateTrn() >>>> pose time 1730869271.28, meas time 1730869270.89 <<<< 2024-11-06T05:01:11Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869271.28 2024-11-06T05:01:11Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869270.89 sec, ping # 0; waiting for INS data more recent than 1730869267.24... 2024-11-06T05:01:11Z- TerrainNav::motionUpdate() - currEstimate = (4061426, 594814.28, 50.15) 2024-11-06T05:01:11Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:01:11Z- TerrainNav:: SubcloudNIS is 0.30, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:01:11Z- TNavPF::Projecting Measurements 2024-11-06T05:01:11Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:01:11Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:01:11Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:01:11Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:01:11Z- TNavFilter::4 good beams. 2024-11-06T05:01:11Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:01:11Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:01:11Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:01:11Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:01:11Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:01:11Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:01:11Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:01:11Z- TerrainMapOctree: (vehN, vehE ) = (4061426.58, 594814.39). 2024-11-06T05:01:11Z- TerrainMapOctree: Min Distance = 187.96. 2024-11-06T05:01:11Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:01:11Z- Forcing Subcloud Comparison 2024-11-06T05:01:11Z- Weighting particles with subcloud comparison 2024-11-06T05:01:11Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:11Z- meanExpectedMeasDiff: 0.455195 delta_rms_squared: 1.643641 alpha: 0.987978 2024-11-06T05:01:11Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:11Z- meanExpectedMeasDiff: 0.086737 delta_rms_squared: 1.090466 alpha: 0.981990 2024-11-06T05:01:11Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:11Z- meanExpectedMeasDiff: 0.619212 delta_rms_squared: 1.297042 alpha: 0.984814 2024-11-06T05:01:11Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:11Z- meanExpectedMeasDiff: 0.255861 delta_rms_squared: 2.152056 alpha: 0.990792 2024-11-06T05:01:11Z- Subcloud NIS: 0.287500 2024-11-06T05:01:11Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:01:11Z- TNavPF:: sumWeights = 0.620675 2024-11-06T05:01:11Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:01:11Z- TNavPF::First Term of Map Covariance Matrix is 0.46 2024-11-06T05:01:11Z- TNavPF::Calculated NIS Value : 0.03 numBeams normalized NIS: 0.01 2024-11-06T05:01:11Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869270.89 sec. 2024-11-06T05:01:11Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:01:11Z- variances([0], [2], and psi_berg) = 0.841848 0.428348 0.000000 2024-11-06T05:01:11Z- publishEstimatesDecoder() - at 1730869271.28 sec 2024-11-06T05:01:11Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:01:11Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061368.58 594817.19 57.67 2024-11-06T05:01:11Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061369.47 594818.74 58.35 lat/lon: 36.69 -121.94 2024-11-06T05:01:11Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.84 0.43 0.69 0.00 2024-11-06T05:01:11Z- AHRS_M2 msg: 1730869271.65 epoch sec; seqNo:3616 2024-11-06T05:01:11Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.43 psi 2024-11-06T05:01:11Z- handleDepth()- Depth_Keller msg: 1730869271.66 epoch sec; seqNo:3847; depth 50.16 2024-11-06T05:01:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869271.69 epoch sec; seqNo:3847; 4061425.87 north; 594814.29 east; 36.69 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T05:01:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869271.28 2024-11-06T05:01:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:11Z- waiting for 1730869275.28, time is 1730869271.69 2024-11-06T05:01:12Z- handleDepth()- Depth_Keller msg: 1730869272.07 epoch sec; seqNo:3848; depth 50.18 2024-11-06T05:01:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869272.08 epoch sec; seqNo:3848; 4061425.49 north; 594814.29 east; 36.69 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T05:01:12Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:01:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869271.28 2024-11-06T05:01:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:12Z- waiting for 1730869275.28, time is 1730869272.08 2024-11-06T05:01:12Z- AHRS_M2 msg: 1730869272.47 epoch sec; seqNo:3617 2024-11-06T05:01:12Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.43 psi 2024-11-06T05:01:12Z- handleDepth()- Depth_Keller msg: 1730869272.47 epoch sec; seqNo:3849; depth 50.18 2024-11-06T05:01:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869272.48 epoch sec; seqNo:3849; 4061425.11 north; 594814.29 east; 36.69 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T05:01:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869271.28 2024-11-06T05:01:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:12Z- waiting for 1730869275.28, time is 1730869272.48 2024-11-06T05:01:12Z- AHRS_M2 msg: 1730869272.86 epoch sec; seqNo:3618 2024-11-06T05:01:12Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.43 psi 2024-11-06T05:01:12Z- handleDepth()- Depth_Keller msg: 1730869272.87 epoch sec; seqNo:3850; depth 50.20 2024-11-06T05:01:12Z- handleDVL() - DVL msg epochMillisec: 1730869272.90 2024-11-06T05:01:12Z- handleDvl() - RDI_Pathfinder msg: 1730869272.90 sec; seqNo:1350; #beams:4 2024-11-06T05:01:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.49 , 38.13 , 40.78 , 42.61 2024-11-06T05:01:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.09 , 0.04 2024-11-06T05:01:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869272.91 epoch sec; seqNo:3850; 4061424.74 north; 594814.30 east; 36.69 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T05:01:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869271.28 2024-11-06T05:01:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:13Z- waiting for 1730869275.28, time is 1730869272.91 2024-11-06T05:01:13Z- AHRS_M2 msg: 1730869273.27 epoch sec; seqNo:3619 2024-11-06T05:01:13Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.43 psi 2024-11-06T05:01:13Z- handleDepth()- Depth_Keller msg: 1730869273.27 epoch sec; seqNo:3851; depth 50.21 2024-11-06T05:01:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869273.31 epoch sec; seqNo:3851; 4061424.72 north; 594813.08 east; 36.69 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T05:01:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869271.28 2024-11-06T05:01:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:13Z- waiting for 1730869275.28, time is 1730869273.31 2024-11-06T05:01:13Z- AHRS_M2 msg: 1730869273.67 epoch sec; seqNo:3620 2024-11-06T05:01:13Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.43 psi 2024-11-06T05:01:13Z- handleDepth()- Depth_Keller msg: 1730869273.67 epoch sec; seqNo:3852; depth 50.23 2024-11-06T05:01:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869273.68 epoch sec; seqNo:3852; 4061424.34 north; 594813.08 east; 36.69 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T05:01:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869271.28 2024-11-06T05:01:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:13Z- waiting for 1730869275.28, time is 1730869273.68 2024-11-06T05:01:14Z- AHRS_M2 msg: 1730869274.08 epoch sec; seqNo:3621 2024-11-06T05:01:14Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.43 psi 2024-11-06T05:01:14Z- handleDepth()- Depth_Keller msg: 1730869274.08 epoch sec; seqNo:3853; depth 50.23 2024-11-06T05:01:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869274.09 epoch sec; seqNo:3853; 4061423.96 north; 594813.09 east; 36.69 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T05:01:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869271.28 2024-11-06T05:01:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:14Z- waiting for 1730869275.28, time is 1730869274.09 2024-11-06T05:01:14Z- AHRS_M2 msg: 1730869274.48 epoch sec; seqNo:3622 2024-11-06T05:01:14Z- AHRS_M2 msg: 0.15 phi; -0.04 theta; 3.43 psi 2024-11-06T05:01:14Z- handleDepth()- Depth_Keller msg: 1730869274.50 epoch sec; seqNo:3854; depth 50.24 2024-11-06T05:01:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869274.51 epoch sec; seqNo:3854; 4061423.59 north; 594813.09 east; 36.69 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T05:01:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869271.28 2024-11-06T05:01:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:14Z- waiting for 1730869275.28, time is 1730869274.51 2024-11-06T05:01:14Z- AHRS_M2 msg: 1730869274.88 epoch sec; seqNo:3623 2024-11-06T05:01:14Z- AHRS_M2 msg: 0.16 phi; -0.04 theta; 3.43 psi 2024-11-06T05:01:14Z- handleDepth()- Depth_Keller msg: 1730869274.89 epoch sec; seqNo:3855; depth 50.24 2024-11-06T05:01:14Z- handleDVL() - DVL msg epochMillisec: 1730869274.92 2024-11-06T05:01:14Z- handleDvl() - RDI_Pathfinder msg: 1730869274.92 sec; seqNo:1351; #beams:4 2024-11-06T05:01:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.69 , 38.13 , 40.98 , 42.61 2024-11-06T05:01:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.06 , 0.06 2024-11-06T05:01:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869274.93 epoch sec; seqNo:3855; 4061423.21 north; 594813.09 east; 36.69 lat; -121.94 lon; 50.24 depth; utmZone: 10 2024-11-06T05:01:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869271.28 2024-11-06T05:01:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:15Z- waiting for 1730869275.28, time is 1730869274.93 2024-11-06T05:01:15Z- AHRS_M2 msg: 1730869275.29 epoch sec; seqNo:3624 2024-11-06T05:01:15Z- AHRS_M2 msg: 0.17 phi; -0.04 theta; 3.44 psi 2024-11-06T05:01:15Z- handleDepth()- Depth_Keller msg: 1730869275.30 epoch sec; seqNo:3856; depth 50.25 2024-11-06T05:01:15Z- handleDVL() - DVL msg epochMillisec: 1730869275.30 2024-11-06T05:01:15Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:15Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:15Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:15Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:01:15Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:01:15Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:01:15Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:01:15Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:01:15Z- handleDvl() - RDI_Pathfinder msg: 1730869275.30 sec; seqNo:1352; #beams:0 2024-11-06T05:01:15Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:01:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.06 , 0.06 2024-11-06T05:01:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869275.32 epoch sec; seqNo:3856; 4061422.83 north; 594813.10 east; 36.69 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T05:01:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869271.28 2024-11-06T05:01:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:15Z- waiting for 1730869275.28, time is 1730869275.32 2024-11-06T05:01:15Z- LcmTrn::updateTrn() >>>> pose time 1730869275.32, meas time 1730869275.30 <<<< 2024-11-06T05:01:15Z- LcmTrn::updateTrn() equating pose 1730869275.30 and meas times 1730869275.30 2024-11-06T05:01:15Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869275.30 2024-11-06T05:01:15Z- TerrainNav::motionUpdate() - currEstimate = (4061423, 594813.10, 50.25) 2024-11-06T05:01:15Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:01:15Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:01:15Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T05:01:15Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T05:01:15Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T05:01:15Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:01:15Z- variances([0], [2], and psi_berg) = 0.845482 0.430846 0.000000 2024-11-06T05:01:15Z- publishEstimatesDecoder() - at 1730869275.30 sec 2024-11-06T05:01:15Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:01:15Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061365.10 594816.07 57.52 2024-11-06T05:01:15Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061366.05 594817.56 58.45 lat/lon: 36.69 -121.94 2024-11-06T05:01:15Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.85 0.43 0.93 0.00 2024-11-06T05:01:15Z- AHRS_M2 msg: 1730869275.69 epoch sec; seqNo:3625 2024-11-06T05:01:15Z- AHRS_M2 msg: 0.18 phi; -0.04 theta; 3.45 psi 2024-11-06T05:01:15Z- handleDepth()- Depth_Keller msg: 1730869275.70 epoch sec; seqNo:3857; depth 50.27 2024-11-06T05:01:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869275.71 epoch sec; seqNo:3857; 4061422.45 north; 594813.10 east; 36.69 lat; -121.94 lon; 50.27 depth; utmZone: 10 2024-11-06T05:01:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869275.30 2024-11-06T05:01:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:15Z- waiting for 1730869279.30, time is 1730869275.71 2024-11-06T05:01:16Z- AHRS_M2 msg: 1730869276.10 epoch sec; seqNo:3626 2024-11-06T05:01:16Z- AHRS_M2 msg: 0.20 phi; -0.04 theta; 3.46 psi 2024-11-06T05:01:16Z- handleDepth()- Depth_Keller msg: 1730869276.10 epoch sec; seqNo:3858; depth 50.28 2024-11-06T05:01:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869276.11 epoch sec; seqNo:3858; 4061422.07 north; 594813.11 east; 36.69 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T05:01:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869275.30 2024-11-06T05:01:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:16Z- waiting for 1730869279.30, time is 1730869276.11 2024-11-06T05:01:16Z- AHRS_M2 msg: 1730869276.50 epoch sec; seqNo:3627 2024-11-06T05:01:16Z- AHRS_M2 msg: 0.22 phi; -0.04 theta; 3.48 psi 2024-11-06T05:01:16Z- handleDepth()- Depth_Keller msg: 1730869276.51 epoch sec; seqNo:3859; depth 50.30 2024-11-06T05:01:16Z- handleDVL() - DVL msg epochMillisec: 1730869276.52 2024-11-06T05:01:16Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:16Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:16Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:16Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:01:16Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:01:16Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:01:16Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:01:16Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:01:16Z- handleDvl() - RDI_Pathfinder msg: 1730869276.52 sec; seqNo:1353; #beams:0 2024-11-06T05:01:16Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:01:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , 0.06 , 0.06 2024-11-06T05:01:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869276.53 epoch sec; seqNo:3859; 4061421.69 north; 594813.11 east; 36.69 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T05:01:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869275.30 2024-11-06T05:01:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:16Z- waiting for 1730869279.30, time is 1730869276.53 2024-11-06T05:01:16Z- AHRS_M2 msg: 1730869276.91 epoch sec; seqNo:3628 2024-11-06T05:01:16Z- AHRS_M2 msg: 0.26 phi; -0.05 theta; 3.50 psi 2024-11-06T05:01:16Z- handleDepth()- Depth_Keller msg: 1730869276.91 epoch sec; seqNo:3860; depth 50.29 2024-11-06T05:01:16Z- handleDVL() - DVL msg epochMillisec: 1730869276.93 2024-11-06T05:01:16Z- handleDvl() - RDI_Pathfinder msg: 1730869276.93 sec; seqNo:1354; #beams:4 2024-11-06T05:01:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 48.32 , 37.72 , 41.19 , 43.02 2024-11-06T05:01:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , 0.04 2024-11-06T05:01:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869276.95 epoch sec; seqNo:3860; 4061421.30 north; 594811.90 east; 36.69 lat; -121.94 lon; 50.29 depth; utmZone: 10 2024-11-06T05:01:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869275.30 2024-11-06T05:01:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:17Z- waiting for 1730869279.30, time is 1730869276.95 2024-11-06T05:01:17Z- AHRS_M2 msg: 1730869277.31 epoch sec; seqNo:3629 2024-11-06T05:01:17Z- AHRS_M2 msg: 0.29 phi; -0.05 theta; 3.53 psi 2024-11-06T05:01:17Z- handleDepth()- Depth_Keller msg: 1730869277.31 epoch sec; seqNo:3861; depth 50.31 2024-11-06T05:01:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869277.33 epoch sec; seqNo:3861; 4061420.92 north; 594811.90 east; 36.69 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T05:01:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869275.30 2024-11-06T05:01:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:17Z- waiting for 1730869279.30, time is 1730869277.33 2024-11-06T05:01:17Z- AHRS_M2 msg: 1730869277.71 epoch sec; seqNo:3630 2024-11-06T05:01:17Z- AHRS_M2 msg: 0.32 phi; -0.06 theta; 3.56 psi 2024-11-06T05:01:17Z- handleDepth()- Depth_Keller msg: 1730869277.72 epoch sec; seqNo:3862; depth 50.32 2024-11-06T05:01:17Z- handleDVL() - DVL msg epochMillisec: 1730869277.72 2024-11-06T05:01:17Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:17Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:17Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:17Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:01:17Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:01:17Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:01:17Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:01:17Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:01:17Z- handleDvl() - RDI_Pathfinder msg: 1730869277.72 sec; seqNo:1355; #beams:0 2024-11-06T05:01:17Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:01:17Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , 0.00 , 0.04 2024-11-06T05:01:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869277.75 epoch sec; seqNo:3862; 4061420.54 north; 594811.90 east; 36.69 lat; -121.94 lon; 50.32 depth; utmZone: 10 2024-11-06T05:01:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869275.30 2024-11-06T05:01:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:17Z- waiting for 1730869279.30, time is 1730869277.75 2024-11-06T05:01:18Z- AHRS_M2 msg: 1730869278.12 epoch sec; seqNo:3631 2024-11-06T05:01:18Z- AHRS_M2 msg: 0.36 phi; -0.07 theta; 3.59 psi 2024-11-06T05:01:18Z- handleDepth()- Depth_Keller msg: 1730869278.12 epoch sec; seqNo:3863; depth 50.33 2024-11-06T05:01:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869278.13 epoch sec; seqNo:3863; 4061420.54 north; 594811.90 east; 36.69 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T05:01:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869275.30 2024-11-06T05:01:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:18Z- waiting for 1730869279.30, time is 1730869278.13 2024-11-06T05:01:18Z- AHRS_M2 msg: 1730869278.52 epoch sec; seqNo:3632 2024-11-06T05:01:18Z- AHRS_M2 msg: 0.39 phi; -0.08 theta; 3.63 psi 2024-11-06T05:01:18Z- handleDepth()- Depth_Keller msg: 1730869278.53 epoch sec; seqNo:3864; depth 50.33 2024-11-06T05:01:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869278.54 epoch sec; seqNo:3864; 4061420.16 north; 594811.91 east; 36.69 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T05:01:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869275.30 2024-11-06T05:01:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:18Z- waiting for 1730869279.30, time is 1730869278.54 2024-11-06T05:01:18Z- AHRS_M2 msg: 1730869278.93 epoch sec; seqNo:3633 2024-11-06T05:01:18Z- AHRS_M2 msg: 0.42 phi; -0.09 theta; 3.66 psi 2024-11-06T05:01:18Z- handleDepth()- Depth_Keller msg: 1730869278.95 epoch sec; seqNo:3865; depth 50.35 2024-11-06T05:01:18Z- handleDVL() - DVL msg epochMillisec: 1730869278.97 2024-11-06T05:01:18Z- handleDvl() - RDI_Pathfinder msg: 1730869278.97 sec; seqNo:1356; #beams:4 2024-11-06T05:01:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 57.29 , 36.09 , 42.61 , 46.28 2024-11-06T05:01:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.00 2024-11-06T05:01:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869279.01 epoch sec; seqNo:3865; 4061419.78 north; 594811.91 east; 36.69 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T05:01:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869275.30 2024-11-06T05:01:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:19Z- waiting for 1730869279.30, time is 1730869279.01 2024-11-06T05:01:19Z- AHRS_M2 msg: 1730869279.33 epoch sec; seqNo:3634 2024-11-06T05:01:19Z- AHRS_M2 msg: 0.43 phi; -0.10 theta; 3.70 psi 2024-11-06T05:01:19Z- handleDepth()- Depth_Keller msg: 1730869279.33 epoch sec; seqNo:3866; depth 50.35 2024-11-06T05:01:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869279.35 epoch sec; seqNo:3866; 4061419.40 north; 594811.92 east; 36.69 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T05:01:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869275.30 2024-11-06T05:01:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:19Z- waiting for 1730869279.30, time is 1730869279.35 2024-11-06T05:01:19Z- LcmTrn::updateTrn() >>>> pose time 1730869279.35, meas time 1730869278.97 <<<< 2024-11-06T05:01:19Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869279.35 2024-11-06T05:01:19Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869278.97 sec, ping # 0; waiting for INS data more recent than 1730869275.30... 2024-11-06T05:01:19Z- TerrainNav::motionUpdate() - currEstimate = (4061419, 594811.92, 50.35) 2024-11-06T05:01:19Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:01:19Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:01:19Z- TNavPF::Projecting Measurements 2024-11-06T05:01:19Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:01:19Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:01:19Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:01:19Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:01:19Z- TNavFilter::4 good beams. 2024-11-06T05:01:19Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:01:19Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:01:19Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:01:19Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:01:19Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:01:19Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:01:19Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:01:19Z- TerrainMapOctree: (vehN, vehE ) = (4061419.72, 594812.03). 2024-11-06T05:01:19Z- TerrainMapOctree: Min Distance = 180.70. 2024-11-06T05:01:19Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:01:19Z- Forcing Subcloud Comparison 2024-11-06T05:01:19Z- Weighting particles with subcloud comparison 2024-11-06T05:01:19Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:19Z- meanExpectedMeasDiff: -3.886038 delta_rms_squared: 3.873395 alpha: 0.994863 2024-11-06T05:01:19Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:19Z- meanExpectedMeasDiff: 0.511431 delta_rms_squared: 1.205005 alpha: 0.983674 2024-11-06T05:01:19Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:19Z- meanExpectedMeasDiff: -0.488201 delta_rms_squared: 2.152154 alpha: 0.990793 2024-11-06T05:01:19Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:19Z- meanExpectedMeasDiff: -2.320058 delta_rms_squared: 2.134006 alpha: 0.990715 2024-11-06T05:01:19Z- Subcloud NIS: 0.290794 2024-11-06T05:01:19Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:01:19Z- TNavPF:: sumWeights = 0.201954 2024-11-06T05:01:19Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:01:19Z- TNavPF::First Term of Map Covariance Matrix is 1.17 2024-11-06T05:01:19Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T05:01:19Z- TerrainNav::Resampling particle filter... 2024-11-06T05:01:19Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869278.97 sec. 2024-11-06T05:01:19Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:01:19Z- variances([0], [2], and psi_berg) = 0.812144 0.420140 0.000000 2024-11-06T05:01:19Z- publishEstimatesDecoder() - at 1730869279.35 sec 2024-11-06T05:01:19Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:01:19Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061365.26 594815.51 58.15 2024-11-06T05:01:19Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061362.59 594816.40 59.26 lat/lon: 36.69 -121.94 2024-11-06T05:01:19Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.81 0.42 0.70 0.00 2024-11-06T05:01:19Z- AHRS_M2 msg: 1730869279.76 epoch sec; seqNo:3635 2024-11-06T05:01:19Z- AHRS_M2 msg: 0.44 phi; -0.11 theta; 3.73 psi 2024-11-06T05:01:19Z- handleDepth()- Depth_Keller msg: 1730869279.79 epoch sec; seqNo:3867; depth 50.37 2024-11-06T05:01:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869279.81 epoch sec; seqNo:3867; 4061419.01 north; 594810.70 east; 36.69 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T05:01:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869279.35 2024-11-06T05:01:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:19Z- waiting for 1730869283.35, time is 1730869279.81 2024-11-06T05:01:20Z- AHRS_M2 msg: 1730869280.18 epoch sec; seqNo:3636 2024-11-06T05:01:20Z- AHRS_M2 msg: 0.45 phi; -0.12 theta; 3.81 psi 2024-11-06T05:01:20Z- handleDepth()- Depth_Keller msg: 1730869280.19 epoch sec; seqNo:3868; depth 50.37 2024-11-06T05:01:20Z- handleDVL() - DVL msg epochMillisec: 1730869280.19 2024-11-06T05:01:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:01:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:01:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:01:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:01:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:01:20Z- handleDvl() - RDI_Pathfinder msg: 1730869280.19 sec; seqNo:1357; #beams:0 2024-11-06T05:01:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:01:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.00 , 0.00 2024-11-06T05:01:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869280.21 epoch sec; seqNo:3868; 4061418.63 north; 594810.70 east; 36.69 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T05:01:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869279.35 2024-11-06T05:01:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:20Z- waiting for 1730869283.35, time is 1730869280.21 2024-11-06T05:01:20Z- AHRS_M2 msg: 1730869280.64 epoch sec; seqNo:3637 2024-11-06T05:01:20Z- AHRS_M2 msg: 0.45 phi; -0.13 theta; 3.84 psi 2024-11-06T05:01:20Z- handleDepth()- Depth_Keller msg: 1730869280.64 epoch sec; seqNo:3869; depth 50.40 2024-11-06T05:01:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869280.66 epoch sec; seqNo:3869; 4061418.63 north; 594810.70 east; 36.69 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T05:01:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869279.35 2024-11-06T05:01:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:20Z- waiting for 1730869283.35, time is 1730869280.66 2024-11-06T05:01:20Z- AHRS_M2 msg: 1730869280.99 epoch sec; seqNo:3638 2024-11-06T05:01:20Z- AHRS_M2 msg: 0.43 phi; -0.13 theta; 3.88 psi 2024-11-06T05:01:20Z- handleDepth()- Depth_Keller msg: 1730869280.99 epoch sec; seqNo:3870; depth 50.44 2024-11-06T05:01:21Z- handleDVL() - DVL msg epochMillisec: 1730869281.01 2024-11-06T05:01:21Z- handleDvl() - RDI_Pathfinder msg: 1730869281.01 sec; seqNo:1358; #beams:4 2024-11-06T05:01:21Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 65.38 , 36.44 , 43.29 , 50.36 2024-11-06T05:01:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , -0.01 2024-11-06T05:01:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869281.02 epoch sec; seqNo:3870; 4061418.25 north; 594810.71 east; 36.69 lat; -121.94 lon; 50.44 depth; utmZone: 10 2024-11-06T05:01:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869279.35 2024-11-06T05:01:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:21Z- waiting for 1730869283.35, time is 1730869281.02 2024-11-06T05:01:21Z- AHRS_M2 msg: 1730869281.39 epoch sec; seqNo:3639 2024-11-06T05:01:21Z- AHRS_M2 msg: 0.43 phi; -0.14 theta; 3.91 psi 2024-11-06T05:01:21Z- handleDepth()- Depth_Keller msg: 1730869281.39 epoch sec; seqNo:3871; depth 50.46 2024-11-06T05:01:21Z- handleDVL() - DVL msg epochMillisec: 1730869281.39 2024-11-06T05:01:21Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:21Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:21Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:21Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:01:21Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:01:21Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:01:21Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:01:21Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:01:21Z- handleDvl() - RDI_Pathfinder msg: 1730869281.39 sec; seqNo:1359; #beams:0 2024-11-06T05:01:21Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:01:21Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , -0.01 2024-11-06T05:01:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869281.42 epoch sec; seqNo:3871; 4061417.88 north; 594810.71 east; 36.69 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T05:01:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869279.35 2024-11-06T05:01:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:21Z- waiting for 1730869283.35, time is 1730869281.42 2024-11-06T05:01:21Z- AHRS_M2 msg: 1730869281.80 epoch sec; seqNo:3640 2024-11-06T05:01:21Z- AHRS_M2 msg: 0.42 phi; -0.14 theta; 3.95 psi 2024-11-06T05:01:21Z- handleDepth()- Depth_Keller msg: 1730869281.80 epoch sec; seqNo:3872; depth 50.48 2024-11-06T05:01:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869281.82 epoch sec; seqNo:3872; 4061417.86 north; 594809.49 east; 36.69 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T05:01:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869279.35 2024-11-06T05:01:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:21Z- waiting for 1730869283.35, time is 1730869281.82 2024-11-06T05:01:22Z- handleDepth()- Depth_Keller msg: 1730869282.22 epoch sec; seqNo:3873; depth 50.52 2024-11-06T05:01:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869282.23 epoch sec; seqNo:3873; 4061417.48 north; 594809.50 east; 36.69 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T05:01:22Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:01:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869279.35 2024-11-06T05:01:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:22Z- waiting for 1730869283.35, time is 1730869282.23 2024-11-06T05:01:22Z- AHRS_M2 msg: 1730869282.61 epoch sec; seqNo:3641 2024-11-06T05:01:22Z- AHRS_M2 msg: 0.41 phi; -0.14 theta; 4.02 psi 2024-11-06T05:01:22Z- handleDepth()- Depth_Keller msg: 1730869282.62 epoch sec; seqNo:3874; depth 50.55 2024-11-06T05:01:22Z- handleDVL() - DVL msg epochMillisec: 1730869282.62 2024-11-06T05:01:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:01:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:01:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:01:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:01:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:01:22Z- handleDvl() - RDI_Pathfinder msg: 1730869282.62 sec; seqNo:1360; #beams:0 2024-11-06T05:01:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:01:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , 0.04 , -0.01 2024-11-06T05:01:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869282.63 epoch sec; seqNo:3874; 4061417.10 north; 594809.50 east; 36.69 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T05:01:22Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869279.35 2024-11-06T05:01:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:22Z- waiting for 1730869283.35, time is 1730869282.63 2024-11-06T05:01:23Z- AHRS_M2 msg: 1730869283.01 epoch sec; seqNo:3642 2024-11-06T05:01:23Z- AHRS_M2 msg: 0.40 phi; -0.14 theta; 4.05 psi 2024-11-06T05:01:23Z- handleDepth()- Depth_Keller msg: 1730869283.01 epoch sec; seqNo:3875; depth 50.58 2024-11-06T05:01:23Z- handleDVL() - DVL msg epochMillisec: 1730869283.04 2024-11-06T05:01:23Z- handleDvl() - RDI_Pathfinder msg: 1730869283.04 sec; seqNo:1361; #beams:4 2024-11-06T05:01:23Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 62.95 , 36.22 , 41.53 , 50.58 2024-11-06T05:01:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.06 , 0.02 2024-11-06T05:01:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869283.05 epoch sec; seqNo:3875; 4061417.10 north; 594809.50 east; 36.69 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T05:01:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869279.35 2024-11-06T05:01:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:23Z- waiting for 1730869283.35, time is 1730869283.05 2024-11-06T05:01:23Z- AHRS_M2 msg: 1730869283.41 epoch sec; seqNo:3643 2024-11-06T05:01:23Z- AHRS_M2 msg: 0.40 phi; -0.14 theta; 4.09 psi 2024-11-06T05:01:23Z- handleDepth()- Depth_Keller msg: 1730869283.42 epoch sec; seqNo:3876; depth 50.63 2024-11-06T05:01:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869283.45 epoch sec; seqNo:3876; 4061416.71 north; 594808.28 east; 36.69 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T05:01:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869279.35 2024-11-06T05:01:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:23Z- waiting for 1730869283.35, time is 1730869283.45 2024-11-06T05:01:23Z- LcmTrn::updateTrn() >>>> pose time 1730869283.45, meas time 1730869283.04 <<<< 2024-11-06T05:01:23Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869283.45 2024-11-06T05:01:23Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869283.04 sec, ping # 0; waiting for INS data more recent than 1730869279.35... 2024-11-06T05:01:23Z- TerrainNav::motionUpdate() - currEstimate = (4061417, 594808.28, 50.63) 2024-11-06T05:01:23Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:01:23Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:01:23Z- TNavPF::Projecting Measurements 2024-11-06T05:01:23Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:01:23Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:01:23Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:01:23Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:01:23Z- TNavFilter::4 good beams. 2024-11-06T05:01:23Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:01:23Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:01:23Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:01:23Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:01:23Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:01:23Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:01:23Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:01:23Z- TerrainMapOctree: (vehN, vehE ) = (4061416.98, 594808.65). 2024-11-06T05:01:23Z- TerrainMapOctree: Min Distance = 176.98. 2024-11-06T05:01:23Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:01:23Z- Forcing Subcloud Comparison 2024-11-06T05:01:23Z- Weighting particles with subcloud comparison 2024-11-06T05:01:23Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:23Z- meanExpectedMeasDiff: 4.046629 delta_rms_squared: 2.611267 alpha: 0.992399 2024-11-06T05:01:23Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:23Z- meanExpectedMeasDiff: 1.403180 delta_rms_squared: 0.960316 alpha: 0.979598 2024-11-06T05:01:23Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:23Z- meanExpectedMeasDiff: 2.145776 delta_rms_squared: 1.974130 alpha: 0.989971 2024-11-06T05:01:23Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:23Z- meanExpectedMeasDiff: 1.902340 delta_rms_squared: 1.802805 alpha: 0.989028 2024-11-06T05:01:23Z- Subcloud NIS: 0.292321 2024-11-06T05:01:23Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:01:23Z- TNavPF:: sumWeights = 0.243737 2024-11-06T05:01:23Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:01:23Z- TNavPF::First Term of Map Covariance Matrix is 1.13 2024-11-06T05:01:23Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.02 2024-11-06T05:01:23Z- TerrainNav::Resampling particle filter... 2024-11-06T05:01:23Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869283.04 sec. 2024-11-06T05:01:23Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:01:23Z- variances([0], [2], and psi_berg) = 0.801457 0.416453 0.000000 2024-11-06T05:01:23Z- publishEstimatesDecoder() - at 1730869283.45 sec 2024-11-06T05:01:23Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:01:23Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061362.66 594811.82 57.83 2024-11-06T05:01:23Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061359.85 594812.78 58.58 lat/lon: 36.69 -121.94 2024-11-06T05:01:23Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.80 0.42 0.68 0.00 2024-11-06T05:01:23Z- AHRS_M2 msg: 1730869283.84 epoch sec; seqNo:3644 2024-11-06T05:01:23Z- AHRS_M2 msg: 0.40 phi; -0.14 theta; 4.13 psi 2024-11-06T05:01:23Z- handleDepth()- Depth_Keller msg: 1730869283.86 epoch sec; seqNo:3877; depth 50.67 2024-11-06T05:01:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869283.87 epoch sec; seqNo:3877; 4061416.71 north; 594808.28 east; 36.69 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T05:01:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869283.45 2024-11-06T05:01:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:24Z- waiting for 1730869287.45, time is 1730869283.87 2024-11-06T05:01:24Z- AHRS_M2 msg: 1730869284.22 epoch sec; seqNo:3645 2024-11-06T05:01:24Z- AHRS_M2 msg: 0.39 phi; -0.13 theta; 4.16 psi 2024-11-06T05:01:24Z- handleDepth()- Depth_Keller msg: 1730869284.23 epoch sec; seqNo:3878; depth 50.73 2024-11-06T05:01:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869284.29 epoch sec; seqNo:3878; 4061416.33 north; 594808.29 east; 36.69 lat; -121.94 lon; 50.73 depth; utmZone: 10 2024-11-06T05:01:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869283.45 2024-11-06T05:01:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:24Z- waiting for 1730869287.45, time is 1730869284.29 2024-11-06T05:01:24Z- AHRS_M2 msg: 1730869284.62 epoch sec; seqNo:3646 2024-11-06T05:01:24Z- AHRS_M2 msg: 0.40 phi; -0.13 theta; 4.20 psi 2024-11-06T05:01:24Z- handleDepth()- Depth_Keller msg: 1730869284.63 epoch sec; seqNo:3879; depth 50.77 2024-11-06T05:01:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869284.65 epoch sec; seqNo:3879; 4061416.33 north; 594808.29 east; 36.69 lat; -121.94 lon; 50.77 depth; utmZone: 10 2024-11-06T05:01:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869283.45 2024-11-06T05:01:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:24Z- waiting for 1730869287.45, time is 1730869284.65 2024-11-06T05:01:25Z- AHRS_M2 msg: 1730869285.03 epoch sec; seqNo:3647 2024-11-06T05:01:25Z- AHRS_M2 msg: 0.40 phi; -0.12 theta; 4.24 psi 2024-11-06T05:01:25Z- handleDepth()- Depth_Keller msg: 1730869285.03 epoch sec; seqNo:3880; depth 50.81 2024-11-06T05:01:25Z- handleDVL() - DVL msg epochMillisec: 1730869285.06 2024-11-06T05:01:25Z- handleDvl() - RDI_Pathfinder msg: 1730869285.06 sec; seqNo:1362; #beams:4 2024-11-06T05:01:25Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 60.08 , 36.00 , 40.86 , 49.03 2024-11-06T05:01:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , 0.02 , 0.06 2024-11-06T05:01:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869285.07 epoch sec; seqNo:3880; 4061416.32 north; 594807.07 east; 36.69 lat; -121.94 lon; 50.81 depth; utmZone: 10 2024-11-06T05:01:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869283.45 2024-11-06T05:01:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:25Z- waiting for 1730869287.45, time is 1730869285.07 2024-11-06T05:01:25Z- AHRS_M2 msg: 1730869285.43 epoch sec; seqNo:3648 2024-11-06T05:01:25Z- AHRS_M2 msg: 0.40 phi; -0.12 theta; 4.28 psi 2024-11-06T05:01:25Z- handleDepth()- Depth_Keller msg: 1730869285.43 epoch sec; seqNo:3881; depth 50.85 2024-11-06T05:01:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869285.45 epoch sec; seqNo:3881; 4061415.94 north; 594807.07 east; 36.69 lat; -121.94 lon; 50.85 depth; utmZone: 10 2024-11-06T05:01:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869283.45 2024-11-06T05:01:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:25Z- waiting for 1730869287.45, time is 1730869285.45 2024-11-06T05:01:25Z- AHRS_M2 msg: 1730869285.84 epoch sec; seqNo:3649 2024-11-06T05:01:25Z- AHRS_M2 msg: 0.40 phi; -0.12 theta; 4.32 psi 2024-11-06T05:01:25Z- handleDepth()- Depth_Keller msg: 1730869285.85 epoch sec; seqNo:3882; depth 50.89 2024-11-06T05:01:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869285.86 epoch sec; seqNo:3882; 4061415.94 north; 594807.07 east; 36.69 lat; -121.94 lon; 50.89 depth; utmZone: 10 2024-11-06T05:01:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869283.45 2024-11-06T05:01:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:26Z- waiting for 1730869287.45, time is 1730869285.86 2024-11-06T05:01:26Z- AHRS_M2 msg: 1730869286.24 epoch sec; seqNo:3650 2024-11-06T05:01:26Z- AHRS_M2 msg: 0.40 phi; -0.11 theta; 4.36 psi 2024-11-06T05:01:26Z- handleDepth()- Depth_Keller msg: 1730869286.25 epoch sec; seqNo:3883; depth 50.95 2024-11-06T05:01:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869286.27 epoch sec; seqNo:3883; 4061415.56 north; 594807.08 east; 36.69 lat; -121.94 lon; 50.95 depth; utmZone: 10 2024-11-06T05:01:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869283.45 2024-11-06T05:01:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:26Z- waiting for 1730869287.45, time is 1730869286.27 2024-11-06T05:01:26Z- AHRS_M2 msg: 1730869286.64 epoch sec; seqNo:3651 2024-11-06T05:01:26Z- AHRS_M2 msg: 0.40 phi; -0.11 theta; 4.40 psi 2024-11-06T05:01:26Z- handleDepth()- Depth_Keller msg: 1730869286.65 epoch sec; seqNo:3884; depth 50.97 2024-11-06T05:01:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869286.66 epoch sec; seqNo:3884; 4061415.55 north; 594805.86 east; 36.69 lat; -121.94 lon; 50.97 depth; utmZone: 10 2024-11-06T05:01:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869283.45 2024-11-06T05:01:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:26Z- waiting for 1730869287.45, time is 1730869286.66 2024-11-06T05:01:27Z- AHRS_M2 msg: 1730869287.05 epoch sec; seqNo:3652 2024-11-06T05:01:27Z- AHRS_M2 msg: 0.40 phi; -0.10 theta; 4.44 psi 2024-11-06T05:01:27Z- handleDepth()- Depth_Keller msg: 1730869287.05 epoch sec; seqNo:3885; depth 51.01 2024-11-06T05:01:27Z- handleDVL() - DVL msg epochMillisec: 1730869287.07 2024-11-06T05:01:27Z- handleDvl() - RDI_Pathfinder msg: 1730869287.07 sec; seqNo:1363; #beams:4 2024-11-06T05:01:27Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 59.86 , 35.78 , 41.08 , 48.15 2024-11-06T05:01:27Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.04 , 0.05 2024-11-06T05:01:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869287.09 epoch sec; seqNo:3885; 4061415.55 north; 594805.86 east; 36.69 lat; -121.94 lon; 51.01 depth; utmZone: 10 2024-11-06T05:01:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869283.45 2024-11-06T05:01:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:27Z- waiting for 1730869287.45, time is 1730869287.09 2024-11-06T05:01:27Z- AHRS_M2 msg: 1730869287.45 epoch sec; seqNo:3653 2024-11-06T05:01:27Z- AHRS_M2 msg: 0.40 phi; -0.10 theta; 4.48 psi 2024-11-06T05:01:27Z- handleDepth()- Depth_Keller msg: 1730869287.46 epoch sec; seqNo:3886; depth 51.04 2024-11-06T05:01:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869287.48 epoch sec; seqNo:3886; 4061415.17 north; 594805.86 east; 36.69 lat; -121.94 lon; 51.04 depth; utmZone: 10 2024-11-06T05:01:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869283.45 2024-11-06T05:01:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:27Z- waiting for 1730869287.45, time is 1730869287.48 2024-11-06T05:01:27Z- LcmTrn::updateTrn() >>>> pose time 1730869287.48, meas time 1730869287.07 <<<< 2024-11-06T05:01:27Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869287.48 2024-11-06T05:01:27Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869287.07 sec, ping # 0; waiting for INS data more recent than 1730869283.45... 2024-11-06T05:01:27Z- TerrainNav::motionUpdate() - currEstimate = (4061415, 594805.86, 51.04) 2024-11-06T05:01:27Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:01:27Z- TerrainNav:: SubcloudNIS is 0.29, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:01:27Z- TNavPF::Projecting Measurements 2024-11-06T05:01:27Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:01:27Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:01:27Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:01:27Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:01:27Z- TNavFilter::4 good beams. 2024-11-06T05:01:27Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:01:27Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:01:27Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:01:27Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:01:27Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:01:27Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:01:27Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:01:27Z- TerrainMapOctree: (vehN, vehE ) = (4061415.33, 594806.11). 2024-11-06T05:01:27Z- TerrainMapOctree: Min Distance = 174.59. 2024-11-06T05:01:27Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:01:27Z- Forcing Subcloud Comparison 2024-11-06T05:01:27Z- Weighting particles with subcloud comparison 2024-11-06T05:01:27Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:27Z- meanExpectedMeasDiff: 0.344764 delta_rms_squared: 2.520782 alpha: 0.992128 2024-11-06T05:01:27Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:27Z- meanExpectedMeasDiff: 0.552264 delta_rms_squared: 0.942846 alpha: 0.979228 2024-11-06T05:01:27Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:27Z- meanExpectedMeasDiff: 1.077620 delta_rms_squared: 1.272977 alpha: 0.984532 2024-11-06T05:01:27Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:27Z- meanExpectedMeasDiff: 0.047073 delta_rms_squared: 1.644220 alpha: 0.987982 2024-11-06T05:01:27Z- Subcloud NIS: 0.249380 2024-11-06T05:01:27Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:01:27Z- TNavPF:: sumWeights = 0.601946 2024-11-06T05:01:27Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:01:27Z- TNavPF::First Term of Map Covariance Matrix is 0.71 2024-11-06T05:01:27Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.03 2024-11-06T05:01:27Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869287.07 sec. 2024-11-06T05:01:27Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:01:27Z- variances([0], [2], and psi_berg) = 0.800394 0.418559 0.000000 2024-11-06T05:01:27Z- publishEstimatesDecoder() - at 1730869287.48 sec 2024-11-06T05:01:27Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:01:27Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061358.88 594810.12 59.03 2024-11-06T05:01:27Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061358.32 594810.36 58.78 lat/lon: 36.69 -121.94 2024-11-06T05:01:27Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.80 0.42 0.66 0.00 2024-11-06T05:01:27Z- AHRS_M2 msg: 1730869287.87 epoch sec; seqNo:3654 2024-11-06T05:01:27Z- AHRS_M2 msg: 0.40 phi; -0.09 theta; 4.52 psi 2024-11-06T05:01:27Z- handleDepth()- Depth_Keller msg: 1730869287.88 epoch sec; seqNo:3887; depth 51.09 2024-11-06T05:01:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869287.91 epoch sec; seqNo:3887; 4061415.16 north; 594804.64 east; 36.69 lat; -121.94 lon; 51.09 depth; utmZone: 10 2024-11-06T05:01:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869287.48 2024-11-06T05:01:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:28Z- waiting for 1730869291.48, time is 1730869287.91 2024-11-06T05:01:28Z- AHRS_M2 msg: 1730869288.26 epoch sec; seqNo:3655 2024-11-06T05:01:28Z- AHRS_M2 msg: 0.40 phi; -0.09 theta; 4.56 psi 2024-11-06T05:01:28Z- handleDepth()- Depth_Keller msg: 1730869288.27 epoch sec; seqNo:3888; depth 51.12 2024-11-06T05:01:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869288.28 epoch sec; seqNo:3888; 4061415.16 north; 594804.64 east; 36.69 lat; -121.94 lon; 51.12 depth; utmZone: 10 2024-11-06T05:01:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869287.48 2024-11-06T05:01:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:28Z- waiting for 1730869291.48, time is 1730869288.28 2024-11-06T05:01:28Z- AHRS_M2 msg: 1730869288.67 epoch sec; seqNo:3656 2024-11-06T05:01:28Z- AHRS_M2 msg: 0.40 phi; -0.08 theta; 4.60 psi 2024-11-06T05:01:28Z- handleDepth()- Depth_Keller msg: 1730869288.68 epoch sec; seqNo:3889; depth 51.14 2024-11-06T05:01:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869288.71 epoch sec; seqNo:3889; 4061415.16 north; 594804.64 east; 36.69 lat; -121.94 lon; 51.14 depth; utmZone: 10 2024-11-06T05:01:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869287.48 2024-11-06T05:01:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:28Z- waiting for 1730869291.48, time is 1730869288.71 2024-11-06T05:01:29Z- AHRS_M2 msg: 1730869289.07 epoch sec; seqNo:3657 2024-11-06T05:01:29Z- AHRS_M2 msg: 0.39 phi; -0.08 theta; 4.64 psi 2024-11-06T05:01:29Z- handleDepth()- Depth_Keller msg: 1730869289.08 epoch sec; seqNo:3890; depth 51.18 2024-11-06T05:01:29Z- handleDVL() - DVL msg epochMillisec: 1730869289.12 2024-11-06T05:01:29Z- handleDvl() - RDI_Pathfinder msg: 1730869289.12 sec; seqNo:1364; #beams:4 2024-11-06T05:01:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 58.97 , 35.34 , 41.53 , 46.83 2024-11-06T05:01:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.04 , -0.00 2024-11-06T05:01:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869289.13 epoch sec; seqNo:3890; 4061415.16 north; 594804.64 east; 36.69 lat; -121.94 lon; 51.18 depth; utmZone: 10 2024-11-06T05:01:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869287.48 2024-11-06T05:01:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:29Z- waiting for 1730869291.48, time is 1730869289.13 2024-11-06T05:01:29Z- AHRS_M2 msg: 1730869289.47 epoch sec; seqNo:3658 2024-11-06T05:01:29Z- AHRS_M2 msg: 0.39 phi; -0.07 theta; 4.69 psi 2024-11-06T05:01:29Z- handleDepth()- Depth_Keller msg: 1730869289.48 epoch sec; seqNo:3891; depth 51.20 2024-11-06T05:01:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869289.49 epoch sec; seqNo:3891; 4061415.14 north; 594803.42 east; 36.69 lat; -121.94 lon; 51.20 depth; utmZone: 10 2024-11-06T05:01:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869287.48 2024-11-06T05:01:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:29Z- waiting for 1730869291.48, time is 1730869289.49 2024-11-06T05:01:29Z- handleDepth()- Depth_Keller msg: 1730869289.90 epoch sec; seqNo:3892; depth 51.25 2024-11-06T05:01:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869289.91 epoch sec; seqNo:3892; 4061415.14 north; 594803.42 east; 36.69 lat; -121.94 lon; 51.25 depth; utmZone: 10 2024-11-06T05:01:30Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:01:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869287.48 2024-11-06T05:01:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:30Z- waiting for 1730869291.48, time is 1730869289.91 2024-11-06T05:01:30Z- AHRS_M2 msg: 1730869290.28 epoch sec; seqNo:3659 2024-11-06T05:01:30Z- AHRS_M2 msg: 0.39 phi; -0.06 theta; 4.77 psi 2024-11-06T05:01:30Z- handleDepth()- Depth_Keller msg: 1730869290.29 epoch sec; seqNo:3893; depth 51.27 2024-11-06T05:01:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869290.30 epoch sec; seqNo:3893; 4061415.14 north; 594803.42 east; 36.69 lat; -121.94 lon; 51.27 depth; utmZone: 10 2024-11-06T05:01:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869287.48 2024-11-06T05:01:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:30Z- waiting for 1730869291.48, time is 1730869290.30 2024-11-06T05:01:30Z- AHRS_M2 msg: 1730869290.73 epoch sec; seqNo:3660 2024-11-06T05:01:30Z- AHRS_M2 msg: 0.39 phi; -0.06 theta; 4.81 psi 2024-11-06T05:01:30Z- handleDepth()- Depth_Keller msg: 1730869290.74 epoch sec; seqNo:3894; depth 51.28 2024-11-06T05:01:30Z- handleDVL() - DVL msg epochMillisec: 1730869290.78 2024-11-06T05:01:30Z- handleDvl() - RDI_Pathfinder msg: 1730869290.78 sec; seqNo:1365; #beams:4 2024-11-06T05:01:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 58.09 , 34.90 , 41.75 , 45.72 2024-11-06T05:01:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.02 , -0.03 2024-11-06T05:01:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869290.79 epoch sec; seqNo:3894; 4061415.13 north; 594802.20 east; 36.69 lat; -121.94 lon; 51.28 depth; utmZone: 10 2024-11-06T05:01:30Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869287.48 2024-11-06T05:01:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:30Z- waiting for 1730869291.48, time is 1730869290.79 2024-11-06T05:01:31Z- AHRS_M2 msg: 1730869291.09 epoch sec; seqNo:3661 2024-11-06T05:01:31Z- AHRS_M2 msg: 0.39 phi; -0.05 theta; 4.85 psi 2024-11-06T05:01:31Z- handleDepth()- Depth_Keller msg: 1730869291.10 epoch sec; seqNo:3895; depth 51.31 2024-11-06T05:01:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869291.11 epoch sec; seqNo:3895; 4061415.13 north; 594802.20 east; 36.69 lat; -121.94 lon; 51.31 depth; utmZone: 10 2024-11-06T05:01:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869287.48 2024-11-06T05:01:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:31Z- waiting for 1730869291.48, time is 1730869291.11 2024-11-06T05:01:31Z- AHRS_M2 msg: 1730869291.49 epoch sec; seqNo:3662 2024-11-06T05:01:31Z- AHRS_M2 msg: 0.39 phi; -0.05 theta; 4.89 psi 2024-11-06T05:01:31Z- handleDepth()- Depth_Keller msg: 1730869291.51 epoch sec; seqNo:3896; depth 51.33 2024-11-06T05:01:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869291.52 epoch sec; seqNo:3896; 4061415.13 north; 594802.20 east; 36.69 lat; -121.94 lon; 51.33 depth; utmZone: 10 2024-11-06T05:01:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869287.48 2024-11-06T05:01:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:31Z- waiting for 1730869291.48, time is 1730869291.52 2024-11-06T05:01:31Z- LcmTrn::updateTrn() >>>> pose time 1730869291.52, meas time 1730869290.78 <<<< 2024-11-06T05:01:31Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869291.52 2024-11-06T05:01:31Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869290.78 sec, ping # 0; waiting for INS data more recent than 1730869287.48... 2024-11-06T05:01:31Z- TerrainNav::motionUpdate() - currEstimate = (4061415, 594802.20, 51.33) 2024-11-06T05:01:31Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:01:31Z- TerrainNav:: SubcloudNIS is 0.25, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:01:31Z- TNavPF::Projecting Measurements 2024-11-06T05:01:31Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:01:31Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:01:31Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:01:31Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:01:31Z- TNavFilter::4 good beams. 2024-11-06T05:01:31Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:01:31Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:01:31Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:01:31Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:01:31Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:01:31Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:01:31Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:01:31Z- TerrainMapOctree: (vehN, vehE ) = (4061415.14, 594802.87). 2024-11-06T05:01:31Z- TerrainMapOctree: Min Distance = 173.39. 2024-11-06T05:01:31Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:01:31Z- Forcing Subcloud Comparison 2024-11-06T05:01:31Z- Weighting particles with subcloud comparison 2024-11-06T05:01:31Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:31Z- meanExpectedMeasDiff: -0.762815 delta_rms_squared: 2.514178 alpha: 0.992108 2024-11-06T05:01:31Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:31Z- meanExpectedMeasDiff: 0.434517 delta_rms_squared: 1.168600 alpha: 0.983173 2024-11-06T05:01:31Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:31Z- meanExpectedMeasDiff: 0.923604 delta_rms_squared: 1.352511 alpha: 0.985428 2024-11-06T05:01:31Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:31Z- meanExpectedMeasDiff: -0.648069 delta_rms_squared: 1.557350 alpha: 0.987321 2024-11-06T05:01:31Z- Subcloud NIS: 0.227285 2024-11-06T05:01:31Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:01:31Z- TNavPF:: sumWeights = 0.536535 2024-11-06T05:01:31Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:01:31Z- TNavPF::First Term of Map Covariance Matrix is 0.69 2024-11-06T05:01:31Z- TNavPF::Calculated NIS Value : 0.17 numBeams normalized NIS: 0.04 2024-11-06T05:01:31Z- TerrainNav::Resampling particle filter... 2024-11-06T05:01:31Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869290.78 sec. 2024-11-06T05:01:31Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:01:31Z- variances([0], [2], and psi_berg) = 0.815130 0.425198 0.000000 2024-11-06T05:01:31Z- publishEstimatesDecoder() - at 1730869291.52 sec 2024-11-06T05:01:31Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:01:31Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061361.18 594805.73 59.77 2024-11-06T05:01:31Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061358.28 594806.70 59.04 lat/lon: 36.69 -121.94 2024-11-06T05:01:31Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.82 0.43 0.66 0.00 2024-11-06T05:01:31Z- AHRS_M2 msg: 1730869291.93 epoch sec; seqNo:3663 2024-11-06T05:01:31Z- AHRS_M2 msg: 0.39 phi; -0.04 theta; 4.92 psi 2024-11-06T05:01:31Z- handleDepth()- Depth_Keller msg: 1730869291.94 epoch sec; seqNo:3897; depth 51.34 2024-11-06T05:01:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869291.97 epoch sec; seqNo:3897; 4061415.13 north; 594802.20 east; 36.69 lat; -121.94 lon; 51.34 depth; utmZone: 10 2024-11-06T05:01:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869291.52 2024-11-06T05:01:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:32Z- waiting for 1730869295.52, time is 1730869291.97 2024-11-06T05:01:32Z- AHRS_M2 msg: 1730869292.30 epoch sec; seqNo:3664 2024-11-06T05:01:32Z- AHRS_M2 msg: 0.39 phi; -0.04 theta; 4.96 psi 2024-11-06T05:01:32Z- handleDepth()- Depth_Keller msg: 1730869292.30 epoch sec; seqNo:3898; depth 51.36 2024-11-06T05:01:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869292.31 epoch sec; seqNo:3898; 4061415.12 north; 594800.98 east; 36.69 lat; -121.94 lon; 51.36 depth; utmZone: 10 2024-11-06T05:01:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869291.52 2024-11-06T05:01:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:32Z- waiting for 1730869295.52, time is 1730869292.31 2024-11-06T05:01:32Z- AHRS_M2 msg: 1730869292.75 epoch sec; seqNo:3665 2024-11-06T05:01:32Z- AHRS_M2 msg: 0.39 phi; -0.04 theta; 5.00 psi 2024-11-06T05:01:32Z- handleDepth()- Depth_Keller msg: 1730869292.75 epoch sec; seqNo:3899; depth 51.37 2024-11-06T05:01:32Z- handleDVL() - DVL msg epochMillisec: 1730869292.78 2024-11-06T05:01:32Z- handleDvl() - RDI_Pathfinder msg: 1730869292.78 sec; seqNo:1366; #beams:4 2024-11-06T05:01:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 57.50 , 35.48 , 42.00 , 44.65 2024-11-06T05:01:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , -0.03 2024-11-06T05:01:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869292.80 epoch sec; seqNo:3899; 4061415.49 north; 594800.97 east; 36.69 lat; -121.94 lon; 51.37 depth; utmZone: 10 2024-11-06T05:01:32Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869291.52 2024-11-06T05:01:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:32Z- waiting for 1730869295.52, time is 1730869292.80 2024-11-06T05:01:33Z- handleDepth()- Depth_Keller msg: 1730869293.13 epoch sec; seqNo:3900; depth 51.38 2024-11-06T05:01:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869293.15 epoch sec; seqNo:3900; 4061415.49 north; 594800.97 east; 36.69 lat; -121.94 lon; 51.38 depth; utmZone: 10 2024-11-06T05:01:33Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:01:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869291.52 2024-11-06T05:01:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:33Z- waiting for 1730869295.52, time is 1730869293.15 2024-11-06T05:01:33Z- AHRS_M2 msg: 1730869293.51 epoch sec; seqNo:3666 2024-11-06T05:01:33Z- AHRS_M2 msg: 0.39 phi; -0.03 theta; 5.08 psi 2024-11-06T05:01:33Z- handleDepth()- Depth_Keller msg: 1730869293.52 epoch sec; seqNo:3901; depth 51.38 2024-11-06T05:01:33Z- handleDVL() - DVL msg epochMillisec: 1730869293.52 2024-11-06T05:01:33Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:33Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:33Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:33Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:01:33Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:01:33Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:01:33Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:01:33Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:01:33Z- handleDvl() - RDI_Pathfinder msg: 1730869293.52 sec; seqNo:1367; #beams:0 2024-11-06T05:01:33Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:01:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.02 , -0.03 2024-11-06T05:01:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869293.53 epoch sec; seqNo:3901; 4061415.49 north; 594800.97 east; 36.69 lat; -121.94 lon; 51.38 depth; utmZone: 10 2024-11-06T05:01:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869291.52 2024-11-06T05:01:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:33Z- waiting for 1730869295.52, time is 1730869293.53 2024-11-06T05:01:33Z- AHRS_M2 msg: 1730869293.92 epoch sec; seqNo:3667 2024-11-06T05:01:33Z- AHRS_M2 msg: 0.39 phi; -0.03 theta; 5.12 psi 2024-11-06T05:01:33Z- handleDepth()- Depth_Keller msg: 1730869293.93 epoch sec; seqNo:3902; depth 51.40 2024-11-06T05:01:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869293.94 epoch sec; seqNo:3902; 4061415.86 north; 594799.75 east; 36.69 lat; -121.94 lon; 51.40 depth; utmZone: 10 2024-11-06T05:01:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869291.52 2024-11-06T05:01:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:34Z- waiting for 1730869295.52, time is 1730869293.94 2024-11-06T05:01:34Z- AHRS_M2 msg: 1730869294.32 epoch sec; seqNo:3668 2024-11-06T05:01:34Z- AHRS_M2 msg: 0.38 phi; -0.03 theta; 5.15 psi 2024-11-06T05:01:34Z- handleDepth()- Depth_Keller msg: 1730869294.32 epoch sec; seqNo:3903; depth 51.40 2024-11-06T05:01:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869294.33 epoch sec; seqNo:3903; 4061415.86 north; 594799.75 east; 36.69 lat; -121.94 lon; 51.40 depth; utmZone: 10 2024-11-06T05:01:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869291.52 2024-11-06T05:01:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:34Z- waiting for 1730869295.52, time is 1730869294.33 2024-11-06T05:01:34Z- AHRS_M2 msg: 1730869294.76 epoch sec; seqNo:3669 2024-11-06T05:01:34Z- AHRS_M2 msg: 0.38 phi; -0.02 theta; 5.19 psi 2024-11-06T05:01:34Z- handleDepth()- Depth_Keller msg: 1730869294.76 epoch sec; seqNo:3904; depth 51.41 2024-11-06T05:01:34Z- handleDVL() - DVL msg epochMillisec: 1730869294.79 2024-11-06T05:01:34Z- handleDvl() - RDI_Pathfinder msg: 1730869294.79 sec; seqNo:1368; #beams:4 2024-11-06T05:01:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 55.87 , 35.27 , 42.00 , 43.84 2024-11-06T05:01:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , -0.03 2024-11-06T05:01:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869294.82 epoch sec; seqNo:3904; 4061415.86 north; 594799.75 east; 36.69 lat; -121.94 lon; 51.41 depth; utmZone: 10 2024-11-06T05:01:34Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869291.52 2024-11-06T05:01:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:34Z- waiting for 1730869295.52, time is 1730869294.82 2024-11-06T05:01:35Z- AHRS_M2 msg: 1730869295.13 epoch sec; seqNo:3670 2024-11-06T05:01:35Z- AHRS_M2 msg: 0.37 phi; -0.02 theta; 5.23 psi 2024-11-06T05:01:35Z- handleDepth()- Depth_Keller msg: 1730869295.13 epoch sec; seqNo:3905; depth 51.43 2024-11-06T05:01:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869295.15 epoch sec; seqNo:3905; 4061416.23 north; 594798.53 east; 36.69 lat; -121.94 lon; 51.43 depth; utmZone: 10 2024-11-06T05:01:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869291.52 2024-11-06T05:01:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:35Z- waiting for 1730869295.52, time is 1730869295.15 2024-11-06T05:01:35Z- AHRS_M2 msg: 1730869295.53 epoch sec; seqNo:3671 2024-11-06T05:01:35Z- AHRS_M2 msg: 0.37 phi; -0.02 theta; 5.27 psi 2024-11-06T05:01:35Z- handleDepth()- Depth_Keller msg: 1730869295.54 epoch sec; seqNo:3906; depth 51.44 2024-11-06T05:01:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869295.55 epoch sec; seqNo:3906; 4061416.23 north; 594798.53 east; 36.69 lat; -121.94 lon; 51.44 depth; utmZone: 10 2024-11-06T05:01:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869291.52 2024-11-06T05:01:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:35Z- waiting for 1730869295.52, time is 1730869295.55 2024-11-06T05:01:35Z- LcmTrn::updateTrn() >>>> pose time 1730869295.55, meas time 1730869294.79 <<<< 2024-11-06T05:01:35Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869295.55 2024-11-06T05:01:35Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869294.79 sec, ping # 0; waiting for INS data more recent than 1730869291.52... 2024-11-06T05:01:35Z- TerrainNav::motionUpdate() - currEstimate = (4061416, 594798.53, 51.44) 2024-11-06T05:01:35Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:01:35Z- TerrainNav:: SubcloudNIS is 0.23, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:01:35Z- TNavPF::Projecting Measurements 2024-11-06T05:01:35Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:01:35Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:01:35Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:01:35Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:01:35Z- TNavFilter::4 good beams. 2024-11-06T05:01:35Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:01:35Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:01:35Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:01:35Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:01:35Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:01:35Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:01:35Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:01:35Z- TerrainMapOctree: (vehN, vehE ) = (4061416.02, 594799.22). 2024-11-06T05:01:35Z- TerrainMapOctree: Min Distance = 173.16. 2024-11-06T05:01:35Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:01:35Z- Forcing Subcloud Comparison 2024-11-06T05:01:35Z- Weighting particles with subcloud comparison 2024-11-06T05:01:35Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:35Z- meanExpectedMeasDiff: 0.336972 delta_rms_squared: 2.365833 alpha: 0.991617 2024-11-06T05:01:35Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:35Z- meanExpectedMeasDiff: 0.875651 delta_rms_squared: 1.302925 alpha: 0.984882 2024-11-06T05:01:35Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:35Z- meanExpectedMeasDiff: 0.861487 delta_rms_squared: 1.371093 alpha: 0.985623 2024-11-06T05:01:35Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:35Z- meanExpectedMeasDiff: -0.230183 delta_rms_squared: 2.411644 alpha: 0.991775 2024-11-06T05:01:35Z- Subcloud NIS: 0.230499 2024-11-06T05:01:35Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:01:35Z- TNavPF:: sumWeights = 0.530462 2024-11-06T05:01:35Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:01:35Z- TNavPF::First Term of Map Covariance Matrix is 0.53 2024-11-06T05:01:35Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.03 2024-11-06T05:01:35Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869294.79 sec. 2024-11-06T05:01:35Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:01:35Z- variances([0], [2], and psi_berg) = 0.837713 0.433049 0.000000 2024-11-06T05:01:35Z- publishEstimatesDecoder() - at 1730869295.55 sec 2024-11-06T05:01:35Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:01:35Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061360.00 594802.57 58.97 2024-11-06T05:01:35Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061359.45 594802.99 58.97 lat/lon: 36.69 -121.94 2024-11-06T05:01:35Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.84 0.43 0.63 0.00 2024-11-06T05:01:35Z- AHRS_M2 msg: 1730869295.94 epoch sec; seqNo:3672 2024-11-06T05:01:35Z- AHRS_M2 msg: 0.37 phi; -0.01 theta; 5.30 psi 2024-11-06T05:01:35Z- handleDepth()- Depth_Keller msg: 1730869295.94 epoch sec; seqNo:3907; depth 51.45 2024-11-06T05:01:35Z- handleDVL() - DVL msg epochMillisec: 1730869295.95 2024-11-06T05:01:35Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:35Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:35Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:35Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:01:35Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:01:35Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:01:35Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:01:35Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:01:35Z- handleDvl() - RDI_Pathfinder msg: 1730869295.95 sec; seqNo:1369; #beams:0 2024-11-06T05:01:35Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:01:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.08 , -0.03 2024-11-06T05:01:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869295.97 epoch sec; seqNo:3907; 4061416.60 north; 594798.52 east; 36.69 lat; -121.94 lon; 51.45 depth; utmZone: 10 2024-11-06T05:01:36Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869295.55 2024-11-06T05:01:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:36Z- waiting for 1730869299.55, time is 1730869295.97 2024-11-06T05:01:36Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T05:01:36Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T05:01:36Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T05:01:36Z- AHRS_M2 msg: 1730869296.33 epoch sec; seqNo:3673 2024-11-06T05:01:36Z- AHRS_M2 msg: 0.37 phi; -0.01 theta; 5.30 psi 2024-11-06T05:01:36Z- handleDepth()- Depth_Keller msg: 1730869296.39 epoch sec; seqNo:3908; depth 51.44 2024-11-06T05:01:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869296.41 epoch sec; seqNo:3908; 4061416.60 north; 594798.52 east; 36.69 lat; -121.94 lon; 51.44 depth; utmZone: 10 2024-11-06T05:01:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869295.55 2024-11-06T05:01:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:36Z- waiting for 1730869299.55, time is 1730869296.41 2024-11-06T05:01:36Z- handleDepth()- Depth_Keller msg: 1730869296.77 epoch sec; seqNo:3909; depth 51.46 2024-11-06T05:01:36Z- handleDVL() - DVL msg epochMillisec: 1730869296.80 2024-11-06T05:01:36Z- handleDvl() - RDI_Pathfinder msg: 1730869296.80 sec; seqNo:1370; #beams:4 2024-11-06T05:01:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 54.85 , 35.27 , 42.21 , 43.22 2024-11-06T05:01:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.10 , -0.03 2024-11-06T05:01:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869296.83 epoch sec; seqNo:3909; 4061416.97 north; 594797.30 east; 36.69 lat; -121.94 lon; 51.46 depth; utmZone: 10 2024-11-06T05:01:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869295.55 2024-11-06T05:01:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:36Z- waiting for 1730869299.55, time is 1730869296.83 2024-11-06T05:01:37Z- AHRS_M2 msg: 1730869297.15 epoch sec; seqNo:3674 2024-11-06T05:01:37Z- AHRS_M2 msg: 0.39 phi; -0.00 theta; 5.45 psi 2024-11-06T05:01:37Z- handleDepth()- Depth_Keller msg: 1730869297.16 epoch sec; seqNo:3910; depth 51.47 2024-11-06T05:01:37Z- handleDVL() - DVL msg epochMillisec: 1730869297.16 2024-11-06T05:01:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:01:37Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:01:37Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:01:37Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:01:37Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:01:37Z- handleDvl() - RDI_Pathfinder msg: 1730869297.16 sec; seqNo:1371; #beams:0 2024-11-06T05:01:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:01:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.10 , -0.03 2024-11-06T05:01:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869297.19 epoch sec; seqNo:3910; 4061416.97 north; 594797.30 east; 36.69 lat; -121.94 lon; 51.47 depth; utmZone: 10 2024-11-06T05:01:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869295.55 2024-11-06T05:01:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:37Z- waiting for 1730869299.55, time is 1730869297.19 2024-11-06T05:01:37Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T05:01:37Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T05:01:37Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T05:01:37Z- AHRS_M2 msg: 1730869297.54 epoch sec; seqNo:3675 2024-11-06T05:01:37Z- AHRS_M2 msg: 0.39 phi; -0.00 theta; 5.45 psi 2024-11-06T05:01:37Z- handleDepth()- Depth_Keller msg: 1730869297.57 epoch sec; seqNo:3911; depth 51.47 2024-11-06T05:01:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869297.59 epoch sec; seqNo:3911; 4061417.35 north; 594797.29 east; 36.69 lat; -121.94 lon; 51.47 depth; utmZone: 10 2024-11-06T05:01:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869295.55 2024-11-06T05:01:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:37Z- waiting for 1730869299.55, time is 1730869297.59 2024-11-06T05:01:37Z- handleDepth()- Depth_Keller msg: 1730869297.98 epoch sec; seqNo:3912; depth 51.47 2024-11-06T05:01:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869297.99 epoch sec; seqNo:3912; 4061417.35 north; 594797.29 east; 36.69 lat; -121.94 lon; 51.47 depth; utmZone: 10 2024-11-06T05:01:38Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:01:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869295.55 2024-11-06T05:01:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:38Z- waiting for 1730869299.55, time is 1730869297.99 2024-11-06T05:01:38Z- AHRS_M2 msg: 1730869298.36 epoch sec; seqNo:3676 2024-11-06T05:01:38Z- AHRS_M2 msg: 0.39 phi; 0.00 theta; 5.56 psi 2024-11-06T05:01:38Z- handleDepth()- Depth_Keller msg: 1730869298.37 epoch sec; seqNo:3913; depth 51.50 2024-11-06T05:01:38Z- handleDVL() - DVL msg epochMillisec: 1730869298.37 2024-11-06T05:01:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:01:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:01:38Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:01:38Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:01:38Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:01:38Z- handleDvl() - RDI_Pathfinder msg: 1730869298.37 sec; seqNo:1372; #beams:0 2024-11-06T05:01:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:01:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.10 , -0.03 2024-11-06T05:01:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869298.38 epoch sec; seqNo:3913; 4061417.73 north; 594797.29 east; 36.69 lat; -121.94 lon; 51.50 depth; utmZone: 10 2024-11-06T05:01:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869295.55 2024-11-06T05:01:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:38Z- waiting for 1730869299.55, time is 1730869298.38 2024-11-06T05:01:38Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T05:01:38Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T05:01:38Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T05:01:38Z- AHRS_M2 msg: 1730869298.76 epoch sec; seqNo:3677 2024-11-06T05:01:38Z- AHRS_M2 msg: 0.39 phi; 0.00 theta; 5.56 psi 2024-11-06T05:01:38Z- handleDepth()- Depth_Keller msg: 1730869298.78 epoch sec; seqNo:3914; depth 51.50 2024-11-06T05:01:38Z- handleDVL() - DVL msg epochMillisec: 1730869298.81 2024-11-06T05:01:38Z- handleDvl() - RDI_Pathfinder msg: 1730869298.81 sec; seqNo:1373; #beams:4 2024-11-06T05:01:38Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 55.05 , 35.27 , 42.61 , 43.02 2024-11-06T05:01:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.09 , -0.02 2024-11-06T05:01:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869298.84 epoch sec; seqNo:3914; 4061418.09 north; 594796.06 east; 36.69 lat; -121.94 lon; 51.50 depth; utmZone: 10 2024-11-06T05:01:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869295.55 2024-11-06T05:01:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:38Z- waiting for 1730869299.55, time is 1730869298.84 2024-11-06T05:01:39Z- AHRS_M2 msg: 1730869299.17 epoch sec; seqNo:3678 2024-11-06T05:01:39Z- AHRS_M2 msg: 0.39 phi; 0.00 theta; 5.63 psi 2024-11-06T05:01:39Z- handleDepth()- Depth_Keller msg: 1730869299.17 epoch sec; seqNo:3915; depth 51.47 2024-11-06T05:01:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869299.19 epoch sec; seqNo:3915; 4061418.09 north; 594796.06 east; 36.69 lat; -121.94 lon; 51.47 depth; utmZone: 10 2024-11-06T05:01:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869295.55 2024-11-06T05:01:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:39Z- waiting for 1730869299.55, time is 1730869299.19 2024-11-06T05:01:39Z- handleDepth()- Depth_Keller msg: 1730869299.59 epoch sec; seqNo:3916; depth 51.49 2024-11-06T05:01:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869299.60 epoch sec; seqNo:3916; 4061418.47 north; 594796.06 east; 36.69 lat; -121.94 lon; 51.49 depth; utmZone: 10 2024-11-06T05:01:39Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:01:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869295.55 2024-11-06T05:01:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:39Z- waiting for 1730869299.55, time is 1730869299.60 2024-11-06T05:01:39Z- LcmTrn::updateTrn() >>>> pose time 1730869299.60, meas time 1730869298.81 <<<< 2024-11-06T05:01:39Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869299.60 2024-11-06T05:01:39Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869298.81 sec, ping # 0; waiting for INS data more recent than 1730869295.55... 2024-11-06T05:01:39Z- TerrainNav::motionUpdate() - currEstimate = (4061418, 594796.06, 51.49) 2024-11-06T05:01:39Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:01:39Z- TerrainNav:: SubcloudNIS is 0.23, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:01:39Z- TNavPF::Projecting Measurements 2024-11-06T05:01:39Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:01:39Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:01:39Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:01:39Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:01:39Z- TNavFilter::4 good beams. 2024-11-06T05:01:39Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:01:39Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:01:39Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:01:39Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:01:39Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:01:39Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:01:39Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:01:39Z- TerrainMapOctree: (vehN, vehE ) = (4061418.03, 594796.54). 2024-11-06T05:01:39Z- TerrainMapOctree: Min Distance = 174.36. 2024-11-06T05:01:39Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:01:39Z- Forcing Subcloud Comparison 2024-11-06T05:01:39Z- Weighting particles with subcloud comparison 2024-11-06T05:01:39Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:39Z- meanExpectedMeasDiff: 0.187368 delta_rms_squared: 2.357555 alpha: 0.991588 2024-11-06T05:01:39Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:39Z- meanExpectedMeasDiff: 0.754268 delta_rms_squared: 1.207292 alpha: 0.983704 2024-11-06T05:01:39Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:39Z- meanExpectedMeasDiff: 0.568316 delta_rms_squared: 1.380479 alpha: 0.985719 2024-11-06T05:01:39Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:39Z- meanExpectedMeasDiff: 0.114670 delta_rms_squared: 2.252763 alpha: 0.991200 2024-11-06T05:01:39Z- Subcloud NIS: 0.228466 2024-11-06T05:01:39Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:01:39Z- TNavPF:: sumWeights = 0.576483 2024-11-06T05:01:39Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:01:39Z- TNavPF::First Term of Map Covariance Matrix is 0.59 2024-11-06T05:01:39Z- TNavPF::Calculated NIS Value : 0.06 numBeams normalized NIS: 0.01 2024-11-06T05:01:39Z- TerrainNav::Resampling particle filter... 2024-11-06T05:01:39Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869298.81 sec. 2024-11-06T05:01:39Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:01:39Z- variances([0], [2], and psi_berg) = 0.828599 0.443082 0.000000 2024-11-06T05:01:39Z- publishEstimatesDecoder() - at 1730869299.60 sec 2024-11-06T05:01:39Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:01:39Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061364.50 594799.54 59.03 2024-11-06T05:01:39Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061361.71 594800.54 58.84 lat/lon: 36.69 -121.94 2024-11-06T05:01:39Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.83 0.44 0.66 0.00 2024-11-06T05:01:39Z- AHRS_M2 msg: 1730869299.98 epoch sec; seqNo:3679 2024-11-06T05:01:39Z- AHRS_M2 msg: 0.39 phi; 0.01 theta; 5.70 psi 2024-11-06T05:01:39Z- handleDepth()- Depth_Keller msg: 1730869299.99 epoch sec; seqNo:3917; depth 51.50 2024-11-06T05:01:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869300.00 epoch sec; seqNo:3917; 4061418.47 north; 594796.06 east; 36.69 lat; -121.94 lon; 51.50 depth; utmZone: 10 2024-11-06T05:01:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869299.60 2024-11-06T05:01:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:40Z- waiting for 1730869303.60, time is 1730869300.00 2024-11-06T05:01:40Z- AHRS_M2 msg: 1730869300.38 epoch sec; seqNo:3680 2024-11-06T05:01:40Z- AHRS_M2 msg: 0.39 phi; 0.01 theta; 5.73 psi 2024-11-06T05:01:40Z- handleDepth()- Depth_Keller msg: 1730869300.39 epoch sec; seqNo:3918; depth 51.49 2024-11-06T05:01:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869300.40 epoch sec; seqNo:3918; 4061418.85 north; 594796.06 east; 36.69 lat; -121.94 lon; 51.49 depth; utmZone: 10 2024-11-06T05:01:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869299.60 2024-11-06T05:01:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:40Z- waiting for 1730869303.60, time is 1730869300.40 2024-11-06T05:01:40Z- AHRS_M2 msg: 1730869300.78 epoch sec; seqNo:3681 2024-11-06T05:01:40Z- AHRS_M2 msg: 0.39 phi; 0.01 theta; 5.77 psi 2024-11-06T05:01:40Z- handleDepth()- Depth_Keller msg: 1730869300.79 epoch sec; seqNo:3919; depth 51.49 2024-11-06T05:01:40Z- handleDVL() - DVL msg epochMillisec: 1730869300.82 2024-11-06T05:01:40Z- handleDvl() - RDI_Pathfinder msg: 1730869300.82 sec; seqNo:1374; #beams:4 2024-11-06T05:01:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 54.44 , 35.27 , 42.41 , 42.61 2024-11-06T05:01:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.06 , -0.01 2024-11-06T05:01:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869300.84 epoch sec; seqNo:3919; 4061419.22 north; 594794.83 east; 36.69 lat; -121.94 lon; 51.49 depth; utmZone: 10 2024-11-06T05:01:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869299.60 2024-11-06T05:01:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:40Z- waiting for 1730869303.60, time is 1730869300.84 2024-11-06T05:01:41Z- AHRS_M2 msg: 1730869301.19 epoch sec; seqNo:3682 2024-11-06T05:01:41Z- AHRS_M2 msg: 0.39 phi; 0.01 theta; 5.80 psi 2024-11-06T05:01:41Z- handleDepth()- Depth_Keller msg: 1730869301.19 epoch sec; seqNo:3920; depth 51.47 2024-11-06T05:01:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869301.21 epoch sec; seqNo:3920; 4061419.59 north; 594794.83 east; 36.69 lat; -121.94 lon; 51.47 depth; utmZone: 10 2024-11-06T05:01:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869299.60 2024-11-06T05:01:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:41Z- waiting for 1730869303.60, time is 1730869301.21 2024-11-06T05:01:41Z- AHRS_M2 msg: 1730869301.59 epoch sec; seqNo:3683 2024-11-06T05:01:41Z- AHRS_M2 msg: 0.38 phi; 0.01 theta; 5.84 psi 2024-11-06T05:01:41Z- handleDepth()- Depth_Keller msg: 1730869301.60 epoch sec; seqNo:3921; depth 51.48 2024-11-06T05:01:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869301.61 epoch sec; seqNo:3921; 4061419.59 north; 594794.83 east; 36.69 lat; -121.94 lon; 51.48 depth; utmZone: 10 2024-11-06T05:01:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869299.60 2024-11-06T05:01:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:41Z- waiting for 1730869303.60, time is 1730869301.61 2024-11-06T05:01:42Z- AHRS_M2 msg: 1730869301.99 epoch sec; seqNo:3684 2024-11-06T05:01:42Z- AHRS_M2 msg: 0.37 phi; 0.01 theta; 5.87 psi 2024-11-06T05:01:42Z- handleDepth()- Depth_Keller msg: 1730869302.00 epoch sec; seqNo:3922; depth 51.47 2024-11-06T05:01:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869302.02 epoch sec; seqNo:3922; 4061419.97 north; 594794.82 east; 36.69 lat; -121.94 lon; 51.47 depth; utmZone: 10 2024-11-06T05:01:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869299.60 2024-11-06T05:01:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:42Z- waiting for 1730869303.60, time is 1730869302.02 2024-11-06T05:01:42Z- AHRS_M2 msg: 1730869302.40 epoch sec; seqNo:3685 2024-11-06T05:01:42Z- AHRS_M2 msg: 0.37 phi; 0.01 theta; 5.90 psi 2024-11-06T05:01:42Z- handleDepth()- Depth_Keller msg: 1730869302.40 epoch sec; seqNo:3923; depth 51.46 2024-11-06T05:01:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869302.41 epoch sec; seqNo:3923; 4061420.35 north; 594794.82 east; 36.69 lat; -121.94 lon; 51.46 depth; utmZone: 10 2024-11-06T05:01:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869299.60 2024-11-06T05:01:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:42Z- waiting for 1730869303.60, time is 1730869302.41 2024-11-06T05:01:42Z- handleDepth()- Depth_Keller msg: 1730869302.83 epoch sec; seqNo:3924; depth 51.45 2024-11-06T05:01:42Z- handleDVL() - DVL msg epochMillisec: 1730869302.85 2024-11-06T05:01:42Z- handleDvl() - RDI_Pathfinder msg: 1730869302.85 sec; seqNo:1375; #beams:4 2024-11-06T05:01:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 53.62 , 35.27 , 42.82 , 42.21 2024-11-06T05:01:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.05 , -0.04 2024-11-06T05:01:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869302.87 epoch sec; seqNo:3924; 4061420.73 north; 594794.81 east; 36.69 lat; -121.94 lon; 51.45 depth; utmZone: 10 2024-11-06T05:01:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869299.60 2024-11-06T05:01:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:42Z- waiting for 1730869303.60, time is 1730869302.87 2024-11-06T05:01:43Z- AHRS_M2 msg: 1730869303.21 epoch sec; seqNo:3686 2024-11-06T05:01:43Z- AHRS_M2 msg: 0.37 phi; 0.02 theta; 5.97 psi 2024-11-06T05:01:43Z- handleDepth()- Depth_Keller msg: 1730869303.21 epoch sec; seqNo:3925; depth 51.45 2024-11-06T05:01:43Z- handleDVL() - DVL msg epochMillisec: 1730869303.22 2024-11-06T05:01:43Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:43Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:43Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:43Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:01:43Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:01:43Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:01:43Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:01:43Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:01:43Z- handleDvl() - RDI_Pathfinder msg: 1730869303.22 sec; seqNo:1376; #beams:0 2024-11-06T05:01:43Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:01:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.05 , -0.04 2024-11-06T05:01:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869303.23 epoch sec; seqNo:3925; 4061420.73 north; 594794.81 east; 36.69 lat; -121.94 lon; 51.45 depth; utmZone: 10 2024-11-06T05:01:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869299.60 2024-11-06T05:01:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:43Z- waiting for 1730869303.60, time is 1730869303.23 2024-11-06T05:01:43Z- AHRS_M2 msg: 1730869303.61 epoch sec; seqNo:3687 2024-11-06T05:01:43Z- AHRS_M2 msg: 0.37 phi; 0.02 theta; 6.01 psi 2024-11-06T05:01:43Z- handleDepth()- Depth_Keller msg: 1730869303.62 epoch sec; seqNo:3926; depth 51.43 2024-11-06T05:01:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869303.64 epoch sec; seqNo:3926; 4061421.11 north; 594794.81 east; 36.69 lat; -121.94 lon; 51.43 depth; utmZone: 10 2024-11-06T05:01:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869299.60 2024-11-06T05:01:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:43Z- waiting for 1730869303.60, time is 1730869303.64 2024-11-06T05:01:43Z- LcmTrn::updateTrn() >>>> pose time 1730869303.64, meas time 1730869303.22 <<<< 2024-11-06T05:01:43Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869303.64 2024-11-06T05:01:43Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T05:01:43Z- TerrainNav::motionUpdate() - currEstimate = (4061421, 594794.81, 51.43) 2024-11-06T05:01:43Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:01:43Z- TerrainNav:: SubcloudNIS is 0.23, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:01:43Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T05:01:43Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T05:01:43Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:01:43Z- variances([0], [2], and psi_berg) = 0.829676 0.443651 0.000000 2024-11-06T05:01:43Z- publishEstimatesDecoder() - at 1730869303.64 sec 2024-11-06T05:01:43Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:01:43Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061367.11 594798.26 58.57 2024-11-06T05:01:43Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061364.35 594799.29 58.78 lat/lon: 36.69 -121.94 2024-11-06T05:01:43Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.83 0.44 0.91 0.00 2024-11-06T05:01:44Z- AHRS_M2 msg: 1730869304.02 epoch sec; seqNo:3688 2024-11-06T05:01:44Z- AHRS_M2 msg: 0.38 phi; 0.02 theta; 6.04 psi 2024-11-06T05:01:44Z- handleDepth()- Depth_Keller msg: 1730869304.02 epoch sec; seqNo:3927; depth 51.42 2024-11-06T05:01:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869304.03 epoch sec; seqNo:3927; 4061421.49 north; 594794.81 east; 36.69 lat; -121.94 lon; 51.42 depth; utmZone: 10 2024-11-06T05:01:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869303.64 2024-11-06T05:01:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:44Z- waiting for 1730869307.64, time is 1730869304.03 2024-11-06T05:01:44Z- AHRS_M2 msg: 1730869304.42 epoch sec; seqNo:3689 2024-11-06T05:01:44Z- AHRS_M2 msg: 0.38 phi; 0.02 theta; 6.07 psi 2024-11-06T05:01:44Z- handleDepth()- Depth_Keller msg: 1730869304.42 epoch sec; seqNo:3928; depth 51.41 2024-11-06T05:01:44Z- handleDVL() - DVL msg epochMillisec: 1730869304.43 2024-11-06T05:01:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:01:44Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:01:44Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:01:44Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:01:44Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:01:44Z- handleDvl() - RDI_Pathfinder msg: 1730869304.43 sec; seqNo:1377; #beams:0 2024-11-06T05:01:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:01:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.05 , -0.04 2024-11-06T05:01:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869304.44 epoch sec; seqNo:3928; 4061421.87 north; 594794.80 east; 36.69 lat; -121.94 lon; 51.41 depth; utmZone: 10 2024-11-06T05:01:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869303.64 2024-11-06T05:01:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:44Z- waiting for 1730869307.64, time is 1730869304.44 2024-11-06T05:01:44Z- AHRS_M2 msg: 1730869304.83 epoch sec; seqNo:3690 2024-11-06T05:01:44Z- AHRS_M2 msg: 0.38 phi; 0.02 theta; 6.10 psi 2024-11-06T05:01:44Z- handleDepth()- Depth_Keller msg: 1730869304.84 epoch sec; seqNo:3929; depth 51.43 2024-11-06T05:01:44Z- handleDVL() - DVL msg epochMillisec: 1730869304.87 2024-11-06T05:01:44Z- handleDvl() - RDI_Pathfinder msg: 1730869304.87 sec; seqNo:1378; #beams:4 2024-11-06T05:01:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 53.62 , 35.27 , 42.61 , 42.00 2024-11-06T05:01:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.06 , -0.06 2024-11-06T05:01:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869304.91 epoch sec; seqNo:3929; 4061422.23 north; 594793.58 east; 36.69 lat; -121.94 lon; 51.43 depth; utmZone: 10 2024-11-06T05:01:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869303.64 2024-11-06T05:01:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:44Z- waiting for 1730869307.64, time is 1730869304.91 2024-11-06T05:01:45Z- AHRS_M2 msg: 1730869305.23 epoch sec; seqNo:3691 2024-11-06T05:01:45Z- AHRS_M2 msg: 0.38 phi; 0.02 theta; 6.14 psi 2024-11-06T05:01:45Z- handleDepth()- Depth_Keller msg: 1730869305.24 epoch sec; seqNo:3930; depth 51.40 2024-11-06T05:01:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869305.27 epoch sec; seqNo:3930; 4061422.23 north; 594793.58 east; 36.69 lat; -121.94 lon; 51.40 depth; utmZone: 10 2024-11-06T05:01:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869303.64 2024-11-06T05:01:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:45Z- waiting for 1730869307.64, time is 1730869305.27 2024-11-06T05:01:45Z- AHRS_M2 msg: 1730869305.63 epoch sec; seqNo:3692 2024-11-06T05:01:45Z- AHRS_M2 msg: 0.39 phi; 0.02 theta; 6.17 psi 2024-11-06T05:01:45Z- handleDepth()- Depth_Keller msg: 1730869305.64 epoch sec; seqNo:3931; depth 51.41 2024-11-06T05:01:45Z- handleDVL() - DVL msg epochMillisec: 1730869305.64 2024-11-06T05:01:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:01:45Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:01:45Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:01:45Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:01:45Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:01:45Z- handleDvl() - RDI_Pathfinder msg: 1730869305.64 sec; seqNo:1379; #beams:0 2024-11-06T05:01:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:01:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.06 , -0.06 2024-11-06T05:01:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869305.65 epoch sec; seqNo:3931; 4061422.61 north; 594793.57 east; 36.69 lat; -121.94 lon; 51.41 depth; utmZone: 10 2024-11-06T05:01:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869303.64 2024-11-06T05:01:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:45Z- waiting for 1730869307.64, time is 1730869305.65 2024-11-06T05:01:46Z- AHRS_M2 msg: 1730869306.04 epoch sec; seqNo:3693 2024-11-06T05:01:46Z- AHRS_M2 msg: 0.39 phi; 0.02 theta; 6.20 psi 2024-11-06T05:01:46Z- handleDepth()- Depth_Keller msg: 1730869306.05 epoch sec; seqNo:3932; depth 51.38 2024-11-06T05:01:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869306.05 epoch sec; seqNo:3932; 4061422.99 north; 594793.57 east; 36.69 lat; -121.94 lon; 51.38 depth; utmZone: 10 2024-11-06T05:01:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869303.64 2024-11-06T05:01:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:46Z- waiting for 1730869307.64, time is 1730869306.05 2024-11-06T05:01:46Z- AHRS_M2 msg: 1730869306.44 epoch sec; seqNo:3694 2024-11-06T05:01:46Z- AHRS_M2 msg: 0.39 phi; 0.02 theta; 6.23 psi 2024-11-06T05:01:46Z- handleDepth()- Depth_Keller msg: 1730869306.45 epoch sec; seqNo:3933; depth 51.38 2024-11-06T05:01:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869306.46 epoch sec; seqNo:3933; 4061423.37 north; 594793.56 east; 36.69 lat; -121.94 lon; 51.38 depth; utmZone: 10 2024-11-06T05:01:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869303.64 2024-11-06T05:01:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:46Z- waiting for 1730869307.64, time is 1730869306.46 2024-11-06T05:01:46Z- AHRS_M2 msg: 1730869306.84 epoch sec; seqNo:3695 2024-11-06T05:01:46Z- AHRS_M2 msg: 0.39 phi; 0.02 theta; 6.26 psi 2024-11-06T05:01:46Z- handleDepth()- Depth_Keller msg: 1730869306.85 epoch sec; seqNo:3934; depth 51.38 2024-11-06T05:01:46Z- handleDVL() - DVL msg epochMillisec: 1730869306.87 2024-11-06T05:01:46Z- handleDvl() - RDI_Pathfinder msg: 1730869306.87 sec; seqNo:1380; #beams:4 2024-11-06T05:01:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 55.05 , 35.27 , 43.02 , 42.00 2024-11-06T05:01:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.09 , -0.07 2024-11-06T05:01:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869306.90 epoch sec; seqNo:3934; 4061423.75 north; 594793.56 east; 36.69 lat; -121.94 lon; 51.38 depth; utmZone: 10 2024-11-06T05:01:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869303.64 2024-11-06T05:01:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:47Z- waiting for 1730869307.64, time is 1730869306.90 2024-11-06T05:01:47Z- AHRS_M2 msg: 1730869307.26 epoch sec; seqNo:3696 2024-11-06T05:01:47Z- AHRS_M2 msg: 0.39 phi; 0.02 theta; 0.01 psi 2024-11-06T05:01:47Z- handleDepth()- Depth_Keller msg: 1730869307.26 epoch sec; seqNo:3935; depth 51.38 2024-11-06T05:01:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869307.28 epoch sec; seqNo:3935; 4061423.75 north; 594793.56 east; 36.69 lat; -121.94 lon; 51.38 depth; utmZone: 10 2024-11-06T05:01:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869303.64 2024-11-06T05:01:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:47Z- waiting for 1730869307.64, time is 1730869307.28 2024-11-06T05:01:47Z- handleDepth()- Depth_Keller msg: 1730869307.67 epoch sec; seqNo:3936; depth 51.36 2024-11-06T05:01:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869307.68 epoch sec; seqNo:3936; 4061424.13 north; 594793.56 east; 36.69 lat; -121.94 lon; 51.36 depth; utmZone: 10 2024-11-06T05:01:47Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:01:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869303.64 2024-11-06T05:01:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:47Z- waiting for 1730869307.64, time is 1730869307.68 2024-11-06T05:01:47Z- LcmTrn::updateTrn() >>>> pose time 1730869307.68, meas time 1730869306.87 <<<< 2024-11-06T05:01:47Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869307.68 2024-11-06T05:01:47Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869306.87 sec, ping # 0; waiting for INS data more recent than 1730869303.64... 2024-11-06T05:01:47Z- TerrainNav::motionUpdate() - currEstimate = (4061424, 594793.56, 51.36) 2024-11-06T05:01:47Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:01:47Z- TerrainNav:: SubcloudNIS is 0.23, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:01:47Z- TNavPF::Projecting Measurements 2024-11-06T05:01:47Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:01:47Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:01:47Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:01:47Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:01:47Z- TNavFilter::4 good beams. 2024-11-06T05:01:47Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:01:47Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:01:47Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:01:47Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:01:47Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:01:47Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:01:47Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:01:47Z- TerrainMapOctree: (vehN, vehE ) = (4061423.52, 594793.81). 2024-11-06T05:01:47Z- TerrainMapOctree: Min Distance = 178.97. 2024-11-06T05:01:47Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:01:47Z- Forcing Subcloud Comparison 2024-11-06T05:01:47Z- Weighting particles with subcloud comparison 2024-11-06T05:01:47Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:47Z- meanExpectedMeasDiff: -0.550801 delta_rms_squared: 3.115845 alpha: 0.993622 2024-11-06T05:01:47Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:47Z- meanExpectedMeasDiff: 0.886662 delta_rms_squared: 1.229405 alpha: 0.983992 2024-11-06T05:01:47Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:47Z- meanExpectedMeasDiff: -1.342123 delta_rms_squared: 1.867972 alpha: 0.989407 2024-11-06T05:01:47Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:47Z- meanExpectedMeasDiff: 0.144552 delta_rms_squared: 1.761506 alpha: 0.988774 2024-11-06T05:01:47Z- Subcloud NIS: 0.225379 2024-11-06T05:01:47Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:01:47Z- TNavPF:: sumWeights = 0.455234 2024-11-06T05:01:47Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:01:47Z- TNavPF::First Term of Map Covariance Matrix is 0.58 2024-11-06T05:01:47Z- TNavPF::Calculated NIS Value : 0.19 numBeams normalized NIS: 0.05 2024-11-06T05:01:47Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869306.87 sec. 2024-11-06T05:01:47Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:01:47Z- variances([0], [2], and psi_berg) = 0.825661 0.453032 0.000000 2024-11-06T05:01:47Z- publishEstimatesDecoder() - at 1730869307.68 sec 2024-11-06T05:01:47Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:01:47Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061366.98 594799.00 59.03 2024-11-06T05:01:47Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061367.36 594798.04 58.80 lat/lon: 36.69 -121.94 2024-11-06T05:01:47Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.83 0.45 0.72 0.00 2024-11-06T05:01:48Z- AHRS_M2 msg: 1730869308.07 epoch sec; seqNo:3697 2024-11-06T05:01:48Z- AHRS_M2 msg: 0.39 phi; 0.03 theta; 0.07 psi 2024-11-06T05:01:48Z- handleDepth()- Depth_Keller msg: 1730869308.10 epoch sec; seqNo:3937; depth 51.36 2024-11-06T05:01:48Z- handleDVL() - DVL msg epochMillisec: 1730869308.10 2024-11-06T05:01:48Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:48Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:48Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:48Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:01:48Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:01:48Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:01:48Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:01:48Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:01:48Z- handleDvl() - RDI_Pathfinder msg: 1730869308.10 sec; seqNo:1381; #beams:0 2024-11-06T05:01:48Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:01:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.78 , -0.09 , -0.07 2024-11-06T05:01:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869308.11 epoch sec; seqNo:3937; 4061424.51 north; 594793.55 east; 36.69 lat; -121.94 lon; 51.36 depth; utmZone: 10 2024-11-06T05:01:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869307.68 2024-11-06T05:01:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:48Z- waiting for 1730869311.68, time is 1730869308.11 2024-11-06T05:01:48Z- AHRS_M2 msg: 1730869308.46 epoch sec; seqNo:3698 2024-11-06T05:01:48Z- AHRS_M2 msg: 0.39 phi; 0.03 theta; 0.10 psi 2024-11-06T05:01:48Z- handleDepth()- Depth_Keller msg: 1730869308.47 epoch sec; seqNo:3938; depth 51.34 2024-11-06T05:01:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869308.47 epoch sec; seqNo:3938; 4061424.88 north; 594793.55 east; 36.69 lat; -121.94 lon; 51.34 depth; utmZone: 10 2024-11-06T05:01:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869307.68 2024-11-06T05:01:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:48Z- waiting for 1730869311.68, time is 1730869308.47 2024-11-06T05:01:48Z- AHRS_M2 msg: 1730869308.86 epoch sec; seqNo:3699 2024-11-06T05:01:48Z- AHRS_M2 msg: 0.38 phi; 0.03 theta; 0.13 psi 2024-11-06T05:01:48Z- handleDepth()- Depth_Keller msg: 1730869308.87 epoch sec; seqNo:3939; depth 51.34 2024-11-06T05:01:48Z- handleDVL() - DVL msg epochMillisec: 1730869308.89 2024-11-06T05:01:48Z- handleDvl() - RDI_Pathfinder msg: 1730869308.89 sec; seqNo:1382; #beams:4 2024-11-06T05:01:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 55.25 , 35.27 , 42.82 , 41.80 2024-11-06T05:01:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.10 , -0.03 2024-11-06T05:01:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869308.93 epoch sec; seqNo:3939; 4061425.26 north; 594793.54 east; 36.69 lat; -121.94 lon; 51.34 depth; utmZone: 10 2024-11-06T05:01:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869307.68 2024-11-06T05:01:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:49Z- waiting for 1730869311.68, time is 1730869308.93 2024-11-06T05:01:49Z- AHRS_M2 msg: 1730869309.27 epoch sec; seqNo:3700 2024-11-06T05:01:49Z- AHRS_M2 msg: 0.38 phi; 0.03 theta; 0.16 psi 2024-11-06T05:01:49Z- handleDepth()- Depth_Keller msg: 1730869309.28 epoch sec; seqNo:3940; depth 51.34 2024-11-06T05:01:49Z- handleDVL() - DVL msg epochMillisec: 1730869309.28 2024-11-06T05:01:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:01:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:01:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:01:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:01:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:01:49Z- handleDvl() - RDI_Pathfinder msg: 1730869309.28 sec; seqNo:1383; #beams:0 2024-11-06T05:01:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:01:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.10 , -0.03 2024-11-06T05:01:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869309.30 epoch sec; seqNo:3940; 4061425.64 north; 594793.54 east; 36.69 lat; -121.94 lon; 51.34 depth; utmZone: 10 2024-11-06T05:01:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869307.68 2024-11-06T05:01:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:49Z- waiting for 1730869311.68, time is 1730869309.30 2024-11-06T05:01:49Z- AHRS_M2 msg: 1730869309.67 epoch sec; seqNo:3701 2024-11-06T05:01:49Z- AHRS_M2 msg: 0.37 phi; 0.04 theta; 0.18 psi 2024-11-06T05:01:49Z- handleDepth()- Depth_Keller msg: 1730869309.68 epoch sec; seqNo:3941; depth 51.34 2024-11-06T05:01:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869309.69 epoch sec; seqNo:3941; 4061425.64 north; 594793.54 east; 36.69 lat; -121.94 lon; 51.34 depth; utmZone: 10 2024-11-06T05:01:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869307.68 2024-11-06T05:01:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:49Z- waiting for 1730869311.68, time is 1730869309.69 2024-11-06T05:01:50Z- AHRS_M2 msg: 1730869310.08 epoch sec; seqNo:3702 2024-11-06T05:01:50Z- AHRS_M2 msg: 0.37 phi; 0.04 theta; 0.21 psi 2024-11-06T05:01:50Z- handleDepth()- Depth_Keller msg: 1730869310.08 epoch sec; seqNo:3942; depth 51.33 2024-11-06T05:01:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869310.10 epoch sec; seqNo:3942; 4061426.02 north; 594793.54 east; 36.69 lat; -121.94 lon; 51.33 depth; utmZone: 10 2024-11-06T05:01:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869307.68 2024-11-06T05:01:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:50Z- waiting for 1730869311.68, time is 1730869310.10 2024-11-06T05:01:50Z- AHRS_M2 msg: 1730869310.48 epoch sec; seqNo:3703 2024-11-06T05:01:50Z- AHRS_M2 msg: 0.36 phi; 0.05 theta; 0.24 psi 2024-11-06T05:01:50Z- handleDepth()- Depth_Keller msg: 1730869310.48 epoch sec; seqNo:3943; depth 51.32 2024-11-06T05:01:50Z- handleDVL() - DVL msg epochMillisec: 1730869310.50 2024-11-06T05:01:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:01:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:01:50Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:01:50Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:01:50Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:01:50Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:01:50Z- handleDvl() - RDI_Pathfinder msg: 1730869310.50 sec; seqNo:1384; #beams:0 2024-11-06T05:01:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:01:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.10 , -0.03 2024-11-06T05:01:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869310.52 epoch sec; seqNo:3943; 4061426.40 north; 594793.53 east; 36.69 lat; -121.94 lon; 51.32 depth; utmZone: 10 2024-11-06T05:01:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869307.68 2024-11-06T05:01:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:50Z- waiting for 1730869311.68, time is 1730869310.52 2024-11-06T05:01:50Z- AHRS_M2 msg: 1730869310.88 epoch sec; seqNo:3704 2024-11-06T05:01:50Z- AHRS_M2 msg: 0.36 phi; 0.05 theta; 0.26 psi 2024-11-06T05:01:50Z- handleDepth()- Depth_Keller msg: 1730869310.89 epoch sec; seqNo:3944; depth 51.31 2024-11-06T05:01:50Z- handleDVL() - DVL msg epochMillisec: 1730869310.93 2024-11-06T05:01:50Z- handleDvl() - RDI_Pathfinder msg: 1730869310.93 sec; seqNo:1385; #beams:4 2024-11-06T05:01:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 53.21 , 35.48 , 43.02 , 41.19 2024-11-06T05:01:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.74 , -0.09 , 0.00 2024-11-06T05:01:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869310.94 epoch sec; seqNo:3944; 4061426.78 north; 594793.53 east; 36.69 lat; -121.94 lon; 51.31 depth; utmZone: 10 2024-11-06T05:01:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869307.68 2024-11-06T05:01:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:51Z- waiting for 1730869311.68, time is 1730869310.94 2024-11-06T05:01:51Z- AHRS_M2 msg: 1730869311.29 epoch sec; seqNo:3705 2024-11-06T05:01:51Z- AHRS_M2 msg: 0.35 phi; 0.06 theta; 0.28 psi 2024-11-06T05:01:51Z- handleDepth()- Depth_Keller msg: 1730869311.29 epoch sec; seqNo:3945; depth 51.30 2024-11-06T05:01:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869311.31 epoch sec; seqNo:3945; 4061427.16 north; 594793.52 east; 36.69 lat; -121.94 lon; 51.30 depth; utmZone: 10 2024-11-06T05:01:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869307.68 2024-11-06T05:01:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:51Z- waiting for 1730869311.68, time is 1730869311.31 2024-11-06T05:01:51Z- handleDepth()- Depth_Keller msg: 1730869311.72 epoch sec; seqNo:3946; depth 51.30 2024-11-06T05:01:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869311.77 epoch sec; seqNo:3946; 4061427.16 north; 594793.52 east; 36.69 lat; -121.94 lon; 51.30 depth; utmZone: 10 2024-11-06T05:01:51Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:01:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869307.68 2024-11-06T05:01:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:51Z- waiting for 1730869311.68, time is 1730869311.77 2024-11-06T05:01:51Z- LcmTrn::updateTrn() >>>> pose time 1730869311.77, meas time 1730869310.93 <<<< 2024-11-06T05:01:51Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869311.77 2024-11-06T05:01:51Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869310.93 sec, ping # 0; waiting for INS data more recent than 1730869307.68... 2024-11-06T05:01:51Z- TerrainNav::motionUpdate() - currEstimate = (4061427, 594793.52, 51.30) 2024-11-06T05:01:51Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:01:51Z- TerrainNav:: SubcloudNIS is 0.23, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:01:51Z- TNavPF::Projecting Measurements 2024-11-06T05:01:51Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:01:51Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:01:51Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:01:51Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:01:51Z- TNavFilter::4 good beams. 2024-11-06T05:01:51Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:01:51Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:01:51Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:01:51Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:01:51Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:01:51Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:01:51Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:01:51Z- TerrainMapOctree: (vehN, vehE ) = (4061426.53, 594793.53). 2024-11-06T05:01:51Z- TerrainMapOctree: Min Distance = 181.82. 2024-11-06T05:01:51Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:01:51Z- Forcing Subcloud Comparison 2024-11-06T05:01:51Z- Weighting particles with subcloud comparison 2024-11-06T05:01:51Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:51Z- meanExpectedMeasDiff: 0.112703 delta_rms_squared: 1.621168 alpha: 0.987814 2024-11-06T05:01:51Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:52Z- meanExpectedMeasDiff: 0.623476 delta_rms_squared: 1.380732 alpha: 0.985722 2024-11-06T05:01:52Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:52Z- meanExpectedMeasDiff: -0.275716 delta_rms_squared: 1.600864 alpha: 0.987661 2024-11-06T05:01:52Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:52Z- meanExpectedMeasDiff: 0.479001 delta_rms_squared: 1.408674 alpha: 0.986001 2024-11-06T05:01:52Z- Subcloud NIS: 0.216007 2024-11-06T05:01:52Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:01:52Z- TNavPF:: sumWeights = 0.604682 2024-11-06T05:01:52Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:01:52Z- TNavPF::First Term of Map Covariance Matrix is 0.47 2024-11-06T05:01:52Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T05:01:52Z- TerrainNav::Resampling particle filter... 2024-11-06T05:01:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869310.93 sec. 2024-11-06T05:01:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:01:52Z- variances([0], [2], and psi_berg) = 0.810516 0.463763 0.000000 2024-11-06T05:01:52Z- publishEstimatesDecoder() - at 1730869311.77 sec 2024-11-06T05:01:52Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:01:52Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061373.16 594797.01 58.66 2024-11-06T05:01:52Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061370.39 594798.01 58.64 lat/lon: 36.69 -121.94 2024-11-06T05:01:52Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.81 0.46 0.73 0.00 2024-11-06T05:01:52Z- AHRS_M2 msg: 1730869312.10 epoch sec; seqNo:3706 2024-11-06T05:01:52Z- AHRS_M2 msg: 0.33 phi; 0.07 theta; 0.32 psi 2024-11-06T05:01:52Z- handleDepth()- Depth_Keller msg: 1730869312.13 epoch sec; seqNo:3947; depth 51.28 2024-11-06T05:01:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869312.15 epoch sec; seqNo:3947; 4061427.55 north; 594794.74 east; 36.69 lat; -121.94 lon; 51.28 depth; utmZone: 10 2024-11-06T05:01:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869311.77 2024-11-06T05:01:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:52Z- waiting for 1730869315.77, time is 1730869312.15 2024-11-06T05:01:52Z- AHRS_M2 msg: 1730869312.50 epoch sec; seqNo:3707 2024-11-06T05:01:52Z- AHRS_M2 msg: 0.32 phi; 0.07 theta; 0.34 psi 2024-11-06T05:01:52Z- handleDepth()- Depth_Keller msg: 1730869312.50 epoch sec; seqNo:3948; depth 51.27 2024-11-06T05:01:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869312.51 epoch sec; seqNo:3948; 4061427.93 north; 594794.73 east; 36.69 lat; -121.94 lon; 51.27 depth; utmZone: 10 2024-11-06T05:01:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869311.77 2024-11-06T05:01:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:52Z- waiting for 1730869315.77, time is 1730869312.51 2024-11-06T05:01:52Z- AHRS_M2 msg: 1730869312.90 epoch sec; seqNo:3708 2024-11-06T05:01:52Z- AHRS_M2 msg: 0.32 phi; 0.08 theta; 0.35 psi 2024-11-06T05:01:52Z- handleDepth()- Depth_Keller msg: 1730869312.91 epoch sec; seqNo:3949; depth 51.25 2024-11-06T05:01:52Z- handleDVL() - DVL msg epochMillisec: 1730869312.94 2024-11-06T05:01:52Z- handleDvl() - RDI_Pathfinder msg: 1730869312.94 sec; seqNo:1386; #beams:4 2024-11-06T05:01:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 51.18 , 35.68 , 43.22 , 40.17 2024-11-06T05:01:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.76 , -0.06 , -0.01 2024-11-06T05:01:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869312.97 epoch sec; seqNo:3949; 4061428.31 north; 594794.73 east; 36.69 lat; -121.94 lon; 51.25 depth; utmZone: 10 2024-11-06T05:01:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869311.77 2024-11-06T05:01:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:53Z- waiting for 1730869315.77, time is 1730869312.97 2024-11-06T05:01:53Z- AHRS_M2 msg: 1730869313.31 epoch sec; seqNo:3709 2024-11-06T05:01:53Z- AHRS_M2 msg: 0.31 phi; 0.09 theta; 0.37 psi 2024-11-06T05:01:53Z- handleDepth()- Depth_Keller msg: 1730869313.31 epoch sec; seqNo:3950; depth 51.23 2024-11-06T05:01:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869313.34 epoch sec; seqNo:3950; 4061428.31 north; 594794.73 east; 36.69 lat; -121.94 lon; 51.23 depth; utmZone: 10 2024-11-06T05:01:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869311.77 2024-11-06T05:01:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:53Z- waiting for 1730869315.77, time is 1730869313.34 2024-11-06T05:01:53Z- AHRS_M2 msg: 1730869313.71 epoch sec; seqNo:3710 2024-11-06T05:01:53Z- AHRS_M2 msg: 0.30 phi; 0.09 theta; 0.38 psi 2024-11-06T05:01:53Z- handleDepth()- Depth_Keller msg: 1730869313.72 epoch sec; seqNo:3951; depth 51.20 2024-11-06T05:01:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869313.74 epoch sec; seqNo:3951; 4061428.69 north; 594794.73 east; 36.69 lat; -121.94 lon; 51.20 depth; utmZone: 10 2024-11-06T05:01:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869311.77 2024-11-06T05:01:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:53Z- waiting for 1730869315.77, time is 1730869313.74 2024-11-06T05:01:54Z- AHRS_M2 msg: 1730869314.12 epoch sec; seqNo:3711 2024-11-06T05:01:54Z- AHRS_M2 msg: 0.28 phi; 0.11 theta; 0.40 psi 2024-11-06T05:01:54Z- handleDepth()- Depth_Keller msg: 1730869314.12 epoch sec; seqNo:3952; depth 51.18 2024-11-06T05:01:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869314.13 epoch sec; seqNo:3952; 4061429.07 north; 594794.72 east; 36.69 lat; -121.94 lon; 51.18 depth; utmZone: 10 2024-11-06T05:01:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869311.77 2024-11-06T05:01:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:54Z- waiting for 1730869315.77, time is 1730869314.13 2024-11-06T05:01:54Z- AHRS_M2 msg: 1730869314.53 epoch sec; seqNo:3712 2024-11-06T05:01:54Z- AHRS_M2 msg: 0.27 phi; 0.12 theta; 0.41 psi 2024-11-06T05:01:54Z- handleDepth()- Depth_Keller msg: 1730869314.53 epoch sec; seqNo:3953; depth 51.15 2024-11-06T05:01:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869314.54 epoch sec; seqNo:3953; 4061429.44 north; 594794.72 east; 36.69 lat; -121.94 lon; 51.15 depth; utmZone: 10 2024-11-06T05:01:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869311.77 2024-11-06T05:01:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:54Z- waiting for 1730869315.77, time is 1730869314.54 2024-11-06T05:01:54Z- AHRS_M2 msg: 1730869314.92 epoch sec; seqNo:3713 2024-11-06T05:01:54Z- AHRS_M2 msg: 0.26 phi; 0.13 theta; 0.42 psi 2024-11-06T05:01:54Z- handleDepth()- Depth_Keller msg: 1730869314.95 epoch sec; seqNo:3954; depth 51.12 2024-11-06T05:01:54Z- handleDVL() - DVL msg epochMillisec: 1730869314.97 2024-11-06T05:01:54Z- handleDvl() - RDI_Pathfinder msg: 1730869314.97 sec; seqNo:1387; #beams:4 2024-11-06T05:01:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 49.14 , 36.29 , 43.84 , 39.15 2024-11-06T05:01:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.05 , -0.04 2024-11-06T05:01:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869314.98 epoch sec; seqNo:3954; 4061429.82 north; 594794.71 east; 36.69 lat; -121.94 lon; 51.12 depth; utmZone: 10 2024-11-06T05:01:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869311.77 2024-11-06T05:01:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:55Z- waiting for 1730869315.77, time is 1730869314.98 2024-11-06T05:01:55Z- AHRS_M2 msg: 1730869315.37 epoch sec; seqNo:3714 2024-11-06T05:01:55Z- AHRS_M2 msg: 0.25 phi; 0.14 theta; 0.43 psi 2024-11-06T05:01:55Z- handleDepth()- Depth_Keller msg: 1730869315.38 epoch sec; seqNo:3955; depth 51.08 2024-11-06T05:01:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869315.39 epoch sec; seqNo:3955; 4061429.82 north; 594794.71 east; 36.69 lat; -121.94 lon; 51.08 depth; utmZone: 10 2024-11-06T05:01:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869311.77 2024-11-06T05:01:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:55Z- waiting for 1730869315.77, time is 1730869315.39 2024-11-06T05:01:55Z- handleDepth()- Depth_Keller msg: 1730869315.79 epoch sec; seqNo:3956; depth 51.03 2024-11-06T05:01:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869315.80 epoch sec; seqNo:3956; 4061430.20 north; 594794.71 east; 36.69 lat; -121.94 lon; 51.03 depth; utmZone: 10 2024-11-06T05:01:55Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:01:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869311.77 2024-11-06T05:01:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:55Z- waiting for 1730869315.77, time is 1730869315.80 2024-11-06T05:01:55Z- LcmTrn::updateTrn() >>>> pose time 1730869315.80, meas time 1730869314.97 <<<< 2024-11-06T05:01:55Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869315.80 2024-11-06T05:01:55Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869314.97 sec, ping # 0; waiting for INS data more recent than 1730869311.77... 2024-11-06T05:01:55Z- TerrainNav::motionUpdate() - currEstimate = (4061430, 594794.71, 51.03) 2024-11-06T05:01:55Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:01:55Z- TerrainNav:: SubcloudNIS is 0.22, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:01:55Z- TNavPF::Projecting Measurements 2024-11-06T05:01:55Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:01:55Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:01:55Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:01:55Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:01:55Z- TNavFilter::4 good beams. 2024-11-06T05:01:55Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:01:55Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:01:55Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:01:55Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:01:55Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:01:55Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:01:55Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:01:55Z- TerrainMapOctree: (vehN, vehE ) = (4061429.57, 594794.46). 2024-11-06T05:01:55Z- TerrainMapOctree: Min Distance = 184.99. 2024-11-06T05:01:55Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:01:56Z- Forcing Subcloud Comparison 2024-11-06T05:01:56Z- Weighting particles with subcloud comparison 2024-11-06T05:01:56Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:56Z- meanExpectedMeasDiff: -0.266410 delta_rms_squared: 2.027385 alpha: 0.990231 2024-11-06T05:01:56Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:56Z- meanExpectedMeasDiff: -0.436049 delta_rms_squared: 1.053186 alpha: 0.981364 2024-11-06T05:01:56Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:56Z- meanExpectedMeasDiff: -1.013407 delta_rms_squared: 1.714776 alpha: 0.988471 2024-11-06T05:01:56Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:01:56Z- meanExpectedMeasDiff: 0.483240 delta_rms_squared: 1.237800 alpha: 0.984099 2024-11-06T05:01:56Z- Subcloud NIS: 0.211252 2024-11-06T05:01:56Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:01:56Z- TNavPF:: sumWeights = 0.576267 2024-11-06T05:01:56Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:01:56Z- TNavPF::First Term of Map Covariance Matrix is 0.52 2024-11-06T05:01:56Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T05:01:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869314.97 sec. 2024-11-06T05:01:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:01:56Z- variances([0], [2], and psi_berg) = 0.785485 0.456055 0.000000 2024-11-06T05:01:56Z- publishEstimatesDecoder() - at 1730869315.80 sec 2024-11-06T05:01:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:01:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061372.62 594799.50 58.16 2024-11-06T05:01:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061373.44 594799.19 58.50 lat/lon: 36.69 -121.94 2024-11-06T05:01:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.79 0.46 0.69 0.00 2024-11-06T05:01:56Z- AHRS_M2 msg: 1730869316.18 epoch sec; seqNo:3715 2024-11-06T05:01:56Z- AHRS_M2 msg: 0.23 phi; 0.15 theta; 0.44 psi 2024-11-06T05:01:56Z- handleDepth()- Depth_Keller msg: 1730869316.20 epoch sec; seqNo:3957; depth 51.01 2024-11-06T05:01:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869316.22 epoch sec; seqNo:3957; 4061430.58 north; 594794.71 east; 36.69 lat; -121.94 lon; 51.01 depth; utmZone: 10 2024-11-06T05:01:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869315.80 2024-11-06T05:01:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:56Z- waiting for 1730869319.80, time is 1730869316.22 2024-11-06T05:01:56Z- AHRS_M2 msg: 1730869316.58 epoch sec; seqNo:3716 2024-11-06T05:01:56Z- AHRS_M2 msg: 0.23 phi; 0.16 theta; 0.44 psi 2024-11-06T05:01:56Z- handleDepth()- Depth_Keller msg: 1730869316.59 epoch sec; seqNo:3958; depth 50.98 2024-11-06T05:01:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869316.60 epoch sec; seqNo:3958; 4061430.96 north; 594794.70 east; 36.69 lat; -121.94 lon; 50.98 depth; utmZone: 10 2024-11-06T05:01:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869315.80 2024-11-06T05:01:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:56Z- waiting for 1730869319.80, time is 1730869316.60 2024-11-06T05:01:56Z- AHRS_M2 msg: 1730869316.99 epoch sec; seqNo:3717 2024-11-06T05:01:56Z- AHRS_M2 msg: 0.22 phi; 0.17 theta; 0.45 psi 2024-11-06T05:01:56Z- handleDepth()- Depth_Keller msg: 1730869316.99 epoch sec; seqNo:3959; depth 50.92 2024-11-06T05:01:57Z- handleDVL() - DVL msg epochMillisec: 1730869317.02 2024-11-06T05:01:57Z- handleDvl() - RDI_Pathfinder msg: 1730869317.02 sec; seqNo:1388; #beams:4 2024-11-06T05:01:57Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 47.51 , 37.11 , 45.26 , 38.33 2024-11-06T05:01:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.08 , -0.05 2024-11-06T05:01:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869317.03 epoch sec; seqNo:3959; 4061430.97 north; 594795.92 east; 36.69 lat; -121.94 lon; 50.92 depth; utmZone: 10 2024-11-06T05:01:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869315.80 2024-11-06T05:01:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:57Z- waiting for 1730869319.80, time is 1730869317.03 2024-11-06T05:01:57Z- AHRS_M2 msg: 1730869317.39 epoch sec; seqNo:3718 2024-11-06T05:01:57Z- AHRS_M2 msg: 0.21 phi; 0.17 theta; 0.45 psi 2024-11-06T05:01:57Z- handleDepth()- Depth_Keller msg: 1730869317.39 epoch sec; seqNo:3960; depth 50.87 2024-11-06T05:01:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869317.41 epoch sec; seqNo:3960; 4061431.35 north; 594795.92 east; 36.69 lat; -121.94 lon; 50.87 depth; utmZone: 10 2024-11-06T05:01:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869315.80 2024-11-06T05:01:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:57Z- waiting for 1730869319.80, time is 1730869317.41 2024-11-06T05:01:57Z- handleDepth()- Depth_Keller msg: 1730869317.82 epoch sec; seqNo:3961; depth 50.82 2024-11-06T05:01:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869317.84 epoch sec; seqNo:3961; 4061431.73 north; 594795.91 east; 36.69 lat; -121.94 lon; 50.82 depth; utmZone: 10 2024-11-06T05:01:57Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:01:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869315.80 2024-11-06T05:01:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:57Z- waiting for 1730869319.80, time is 1730869317.84 2024-11-06T05:01:58Z- AHRS_M2 msg: 1730869318.20 epoch sec; seqNo:3719 2024-11-06T05:01:58Z- AHRS_M2 msg: 0.21 phi; 0.18 theta; 0.46 psi 2024-11-06T05:01:58Z- handleDepth()- Depth_Keller msg: 1730869318.20 epoch sec; seqNo:3962; depth 50.76 2024-11-06T05:01:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869318.21 epoch sec; seqNo:3962; 4061432.11 north; 594795.91 east; 36.69 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T05:01:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869315.80 2024-11-06T05:01:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:58Z- waiting for 1730869319.80, time is 1730869318.21 2024-11-06T05:01:58Z- AHRS_M2 msg: 1730869318.60 epoch sec; seqNo:3720 2024-11-06T05:01:58Z- AHRS_M2 msg: 0.21 phi; 0.18 theta; 0.47 psi 2024-11-06T05:01:58Z- handleDepth()- Depth_Keller msg: 1730869318.60 epoch sec; seqNo:3963; depth 50.71 2024-11-06T05:01:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869318.62 epoch sec; seqNo:3963; 4061432.11 north; 594795.91 east; 36.69 lat; -121.94 lon; 50.71 depth; utmZone: 10 2024-11-06T05:01:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869315.80 2024-11-06T05:01:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:58Z- waiting for 1730869319.80, time is 1730869318.62 2024-11-06T05:01:59Z- AHRS_M2 msg: 1730869319.00 epoch sec; seqNo:3721 2024-11-06T05:01:59Z- AHRS_M2 msg: 0.21 phi; 0.19 theta; 0.47 psi 2024-11-06T05:01:59Z- handleDepth()- Depth_Keller msg: 1730869319.00 epoch sec; seqNo:3964; depth 50.66 2024-11-06T05:01:59Z- handleDVL() - DVL msg epochMillisec: 1730869319.04 2024-11-06T05:01:59Z- handleDvl() - RDI_Pathfinder msg: 1730869319.04 sec; seqNo:1389; #beams:4 2024-11-06T05:01:59Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.69 , 37.92 , 46.08 , 37.72 2024-11-06T05:01:59Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.08 , -0.03 2024-11-06T05:01:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869319.05 epoch sec; seqNo:3964; 4061432.49 north; 594795.90 east; 36.69 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T05:01:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:01:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869315.80 2024-11-06T05:01:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:59Z- waiting for 1730869319.80, time is 1730869319.05 2024-11-06T05:01:59Z- AHRS_M2 msg: 1730869319.40 epoch sec; seqNo:3722 2024-11-06T05:01:59Z- AHRS_M2 msg: 0.20 phi; 0.19 theta; 0.47 psi 2024-11-06T05:01:59Z- handleDepth()- Depth_Keller msg: 1730869319.41 epoch sec; seqNo:3965; depth 50.58 2024-11-06T05:01:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869319.42 epoch sec; seqNo:3965; 4061432.87 north; 594795.90 east; 36.69 lat; -121.94 lon; 50.58 depth; utmZone: 10 2024-11-06T05:01:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869315.80 2024-11-06T05:01:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:59Z- waiting for 1730869319.80, time is 1730869319.42 2024-11-06T05:01:59Z- AHRS_M2 msg: 1730869319.81 epoch sec; seqNo:3723 2024-11-06T05:01:59Z- AHRS_M2 msg: 0.20 phi; 0.19 theta; 0.48 psi 2024-11-06T05:01:59Z- handleDepth()- Depth_Keller msg: 1730869319.82 epoch sec; seqNo:3966; depth 50.54 2024-11-06T05:01:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869319.83 epoch sec; seqNo:3966; 4061433.25 north; 594795.90 east; 36.69 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T05:01:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:01:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869315.80 2024-11-06T05:01:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:01:59Z- waiting for 1730869319.80, time is 1730869319.83 2024-11-06T05:01:59Z- LcmTrn::updateTrn() >>>> pose time 1730869319.83, meas time 1730869319.04 <<<< 2024-11-06T05:01:59Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869319.83 2024-11-06T05:01:59Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869319.04 sec, ping # 0; waiting for INS data more recent than 1730869315.80... 2024-11-06T05:01:59Z- TerrainNav::motionUpdate() - currEstimate = (4061433, 594795.90, 50.54) 2024-11-06T05:01:59Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:01:59Z- TerrainNav:: SubcloudNIS is 0.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:01:59Z- TNavPF::Projecting Measurements 2024-11-06T05:01:59Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:01:59Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:01:59Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:01:59Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:01:59Z- TNavFilter::4 good beams. 2024-11-06T05:01:59Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:01:59Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:01:59Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:01:59Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:01:59Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:01:59Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:01:59Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:02:00Z- TerrainMapOctree: (vehN, vehE ) = (4061432.64, 594795.66). 2024-11-06T05:02:00Z- TerrainMapOctree: Min Distance = 188.27. 2024-11-06T05:02:00Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:02:00Z- Forcing Subcloud Comparison 2024-11-06T05:02:00Z- Weighting particles with subcloud comparison 2024-11-06T05:02:00Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:00Z- meanExpectedMeasDiff: -0.589760 delta_rms_squared: 1.733942 alpha: 0.988597 2024-11-06T05:02:00Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:00Z- meanExpectedMeasDiff: -0.233473 delta_rms_squared: 1.063009 alpha: 0.981533 2024-11-06T05:02:00Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:00Z- meanExpectedMeasDiff: -1.921580 delta_rms_squared: 1.687662 alpha: 0.988288 2024-11-06T05:02:00Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:00Z- meanExpectedMeasDiff: 0.242216 delta_rms_squared: 1.085737 alpha: 0.981913 2024-11-06T05:02:00Z- Subcloud NIS: 0.209310 2024-11-06T05:02:00Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:02:00Z- TNavPF:: sumWeights = 0.493641 2024-11-06T05:02:00Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:02:00Z- TNavPF::First Term of Map Covariance Matrix is 0.40 2024-11-06T05:02:00Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.05 2024-11-06T05:02:00Z- TerrainNav::Resampling particle filter... 2024-11-06T05:02:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869319.04 sec. 2024-11-06T05:02:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:02:00Z- variances([0], [2], and psi_berg) = 0.800500 0.468819 0.000000 2024-11-06T05:02:00Z- publishEstimatesDecoder() - at 1730869319.83 sec 2024-11-06T05:02:00Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:02:00Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061379.26 594799.40 58.39 2024-11-06T05:02:00Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061376.48 594800.38 58.28 lat/lon: 36.69 -121.94 2024-11-06T05:02:00Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.80 0.47 0.68 0.00 2024-11-06T05:02:00Z- AHRS_M2 msg: 1730869320.21 epoch sec; seqNo:3724 2024-11-06T05:02:00Z- AHRS_M2 msg: 0.19 phi; 0.20 theta; 0.48 psi 2024-11-06T05:02:00Z- handleDepth()- Depth_Keller msg: 1730869320.23 epoch sec; seqNo:3967; depth 50.48 2024-11-06T05:02:00Z- handleDVL() - DVL msg epochMillisec: 1730869320.23 2024-11-06T05:02:00Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:00Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:00Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:00Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:02:00Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:02:00Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:02:00Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:02:00Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:02:00Z- handleDvl() - RDI_Pathfinder msg: 1730869320.23 sec; seqNo:1390; #beams:0 2024-11-06T05:02:00Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:02:00Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.08 , -0.03 2024-11-06T05:02:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869320.26 epoch sec; seqNo:3967; 4061433.62 north; 594795.89 east; 36.69 lat; -121.94 lon; 50.48 depth; utmZone: 10 2024-11-06T05:02:00Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869319.83 2024-11-06T05:02:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:00Z- waiting for 1730869323.83, time is 1730869320.26 2024-11-06T05:02:00Z- AHRS_M2 msg: 1730869320.62 epoch sec; seqNo:3725 2024-11-06T05:02:00Z- AHRS_M2 msg: 0.19 phi; 0.20 theta; 0.48 psi 2024-11-06T05:02:00Z- handleDepth()- Depth_Keller msg: 1730869320.62 epoch sec; seqNo:3968; depth 50.42 2024-11-06T05:02:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869320.63 epoch sec; seqNo:3968; 4061433.62 north; 594795.89 east; 36.69 lat; -121.94 lon; 50.42 depth; utmZone: 10 2024-11-06T05:02:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869319.83 2024-11-06T05:02:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:00Z- waiting for 1730869323.83, time is 1730869320.63 2024-11-06T05:02:01Z- AHRS_M2 msg: 1730869321.02 epoch sec; seqNo:3726 2024-11-06T05:02:01Z- AHRS_M2 msg: 0.18 phi; 0.20 theta; 0.48 psi 2024-11-06T05:02:01Z- handleDepth()- Depth_Keller msg: 1730869321.03 epoch sec; seqNo:3969; depth 50.35 2024-11-06T05:02:01Z- handleDVL() - DVL msg epochMillisec: 1730869321.06 2024-11-06T05:02:01Z- handleDvl() - RDI_Pathfinder msg: 1730869321.06 sec; seqNo:1391; #beams:4 2024-11-06T05:02:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 46.28 , 38.74 , 47.51 , 37.72 2024-11-06T05:02:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.79 , -0.09 , -0.01 2024-11-06T05:02:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869321.07 epoch sec; seqNo:3969; 4061434.00 north; 594795.89 east; 36.69 lat; -121.94 lon; 50.35 depth; utmZone: 10 2024-11-06T05:02:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869319.83 2024-11-06T05:02:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:01Z- waiting for 1730869323.83, time is 1730869321.07 2024-11-06T05:02:01Z- AHRS_M2 msg: 1730869321.42 epoch sec; seqNo:3727 2024-11-06T05:02:01Z- AHRS_M2 msg: 0.18 phi; 0.20 theta; 0.49 psi 2024-11-06T05:02:01Z- handleDepth()- Depth_Keller msg: 1730869321.43 epoch sec; seqNo:3970; depth 50.28 2024-11-06T05:02:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869321.45 epoch sec; seqNo:3970; 4061434.40 north; 594797.10 east; 36.69 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T05:02:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869319.83 2024-11-06T05:02:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:01Z- waiting for 1730869323.83, time is 1730869321.45 2024-11-06T05:02:01Z- AHRS_M2 msg: 1730869321.84 epoch sec; seqNo:3728 2024-11-06T05:02:01Z- AHRS_M2 msg: 0.17 phi; 0.20 theta; 0.49 psi 2024-11-06T05:02:01Z- handleDepth()- Depth_Keller msg: 1730869321.84 epoch sec; seqNo:3971; depth 50.21 2024-11-06T05:02:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869321.85 epoch sec; seqNo:3971; 4061434.78 north; 594797.10 east; 36.69 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T05:02:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869319.83 2024-11-06T05:02:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:01Z- waiting for 1730869323.83, time is 1730869321.85 2024-11-06T05:02:02Z- AHRS_M2 msg: 1730869322.23 epoch sec; seqNo:3729 2024-11-06T05:02:02Z- AHRS_M2 msg: 0.17 phi; 0.20 theta; 0.49 psi 2024-11-06T05:02:02Z- handleDepth()- Depth_Keller msg: 1730869322.24 epoch sec; seqNo:3972; depth 50.13 2024-11-06T05:02:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869322.26 epoch sec; seqNo:3972; 4061434.78 north; 594797.10 east; 36.69 lat; -121.94 lon; 50.13 depth; utmZone: 10 2024-11-06T05:02:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869319.83 2024-11-06T05:02:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:02Z- waiting for 1730869323.83, time is 1730869322.26 2024-11-06T05:02:02Z- AHRS_M2 msg: 1730869322.64 epoch sec; seqNo:3730 2024-11-06T05:02:02Z- AHRS_M2 msg: 0.17 phi; 0.20 theta; 0.49 psi 2024-11-06T05:02:02Z- handleDepth()- Depth_Keller msg: 1730869322.64 epoch sec; seqNo:3973; depth 50.08 2024-11-06T05:02:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869322.65 epoch sec; seqNo:3973; 4061435.15 north; 594797.10 east; 36.69 lat; -121.94 lon; 50.08 depth; utmZone: 10 2024-11-06T05:02:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869319.83 2024-11-06T05:02:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:02Z- waiting for 1730869323.83, time is 1730869322.65 2024-11-06T05:02:03Z- AHRS_M2 msg: 1730869323.05 epoch sec; seqNo:3731 2024-11-06T05:02:03Z- AHRS_M2 msg: 0.17 phi; 0.20 theta; 0.50 psi 2024-11-06T05:02:03Z- handleDepth()- Depth_Keller msg: 1730869323.05 epoch sec; seqNo:3974; depth 50.01 2024-11-06T05:02:03Z- handleDVL() - DVL msg epochMillisec: 1730869323.07 2024-11-06T05:02:03Z- handleDvl() - RDI_Pathfinder msg: 1730869323.07 sec; seqNo:1392; #beams:4 2024-11-06T05:02:03Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.88 , 39.35 , 47.51 , 37.72 2024-11-06T05:02:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.80 , -0.08 , 0.01 2024-11-06T05:02:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869323.10 epoch sec; seqNo:3974; 4061435.53 north; 594797.09 east; 36.69 lat; -121.94 lon; 50.01 depth; utmZone: 10 2024-11-06T05:02:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869319.83 2024-11-06T05:02:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:03Z- waiting for 1730869323.83, time is 1730869323.10 2024-11-06T05:02:03Z- AHRS_M2 msg: 1730869323.44 epoch sec; seqNo:3732 2024-11-06T05:02:03Z- AHRS_M2 msg: 0.17 phi; 0.19 theta; 0.50 psi 2024-11-06T05:02:03Z- handleDepth()- Depth_Keller msg: 1730869323.45 epoch sec; seqNo:3975; depth 49.93 2024-11-06T05:02:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869323.47 epoch sec; seqNo:3975; 4061435.91 north; 594797.09 east; 36.69 lat; -121.94 lon; 49.93 depth; utmZone: 10 2024-11-06T05:02:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869319.83 2024-11-06T05:02:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:03Z- waiting for 1730869323.83, time is 1730869323.47 2024-11-06T05:02:03Z- AHRS_M2 msg: 1730869323.85 epoch sec; seqNo:3733 2024-11-06T05:02:03Z- AHRS_M2 msg: 0.17 phi; 0.19 theta; 0.50 psi 2024-11-06T05:02:03Z- handleDepth()- Depth_Keller msg: 1730869323.85 epoch sec; seqNo:3976; depth 49.87 2024-11-06T05:02:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869323.87 epoch sec; seqNo:3976; 4061435.91 north; 594797.09 east; 36.69 lat; -121.94 lon; 49.87 depth; utmZone: 10 2024-11-06T05:02:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869319.83 2024-11-06T05:02:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:04Z- waiting for 1730869323.83, time is 1730869323.87 2024-11-06T05:02:04Z- LcmTrn::updateTrn() >>>> pose time 1730869323.87, meas time 1730869323.07 <<<< 2024-11-06T05:02:04Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869323.87 2024-11-06T05:02:04Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869323.07 sec, ping # 0; waiting for INS data more recent than 1730869319.83... 2024-11-06T05:02:04Z- TerrainNav::motionUpdate() - currEstimate = (4061436, 594797.09, 49.87) 2024-11-06T05:02:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:02:04Z- TerrainNav:: SubcloudNIS is 0.21, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:02:04Z- TNavPF::Projecting Measurements 2024-11-06T05:02:04Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:02:04Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:02:04Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:02:04Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:02:04Z- TNavFilter::4 good beams. 2024-11-06T05:02:04Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:02:04Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:02:04Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:02:04Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:02:04Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:02:04Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:02:04Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:02:04Z- TerrainMapOctree: (vehN, vehE ) = (4061435.38, 594796.85). 2024-11-06T05:02:04Z- TerrainMapOctree: Min Distance = 191.21. 2024-11-06T05:02:04Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:02:04Z- Forcing Subcloud Comparison 2024-11-06T05:02:04Z- Weighting particles with subcloud comparison 2024-11-06T05:02:04Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:04Z- meanExpectedMeasDiff: -0.568504 delta_rms_squared: 1.632703 alpha: 0.987899 2024-11-06T05:02:04Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:04Z- meanExpectedMeasDiff: 0.271083 delta_rms_squared: 1.091140 alpha: 0.982000 2024-11-06T05:02:04Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:04Z- meanExpectedMeasDiff: -1.044658 delta_rms_squared: 1.688694 alpha: 0.988295 2024-11-06T05:02:04Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:04Z- meanExpectedMeasDiff: 0.218124 delta_rms_squared: 1.062773 alpha: 0.981529 2024-11-06T05:02:04Z- Subcloud NIS: 0.199704 2024-11-06T05:02:04Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:02:04Z- TNavPF:: sumWeights = 0.598615 2024-11-06T05:02:04Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:02:04Z- TNavPF::First Term of Map Covariance Matrix is 0.47 2024-11-06T05:02:04Z- TNavPF::Calculated NIS Value : 0.13 numBeams normalized NIS: 0.03 2024-11-06T05:02:04Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869323.07 sec. 2024-11-06T05:02:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:02:04Z- variances([0], [2], and psi_berg) = 0.801032 0.471637 0.000000 2024-11-06T05:02:04Z- publishEstimatesDecoder() - at 1730869323.87 sec 2024-11-06T05:02:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:02:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061378.81 594801.21 57.84 2024-11-06T05:02:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061379.14 594801.57 57.73 lat/lon: 36.69 -121.94 2024-11-06T05:02:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.80 0.47 0.70 0.00 2024-11-06T05:02:04Z- AHRS_M2 msg: 1730869324.26 epoch sec; seqNo:3734 2024-11-06T05:02:04Z- AHRS_M2 msg: 0.16 phi; 0.19 theta; 0.51 psi 2024-11-06T05:02:04Z- handleDepth()- Depth_Keller msg: 1730869324.27 epoch sec; seqNo:3977; depth 49.81 2024-11-06T05:02:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869324.29 epoch sec; seqNo:3977; 4061436.29 north; 594797.08 east; 36.69 lat; -121.94 lon; 49.81 depth; utmZone: 10 2024-11-06T05:02:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869323.87 2024-11-06T05:02:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:04Z- waiting for 1730869327.87, time is 1730869324.29 2024-11-06T05:02:04Z- AHRS_M2 msg: 1730869324.67 epoch sec; seqNo:3735 2024-11-06T05:02:04Z- AHRS_M2 msg: 0.16 phi; 0.19 theta; 0.51 psi 2024-11-06T05:02:04Z- handleDepth()- Depth_Keller msg: 1730869324.67 epoch sec; seqNo:3978; depth 49.73 2024-11-06T05:02:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869324.72 epoch sec; seqNo:3978; 4061436.67 north; 594797.08 east; 36.69 lat; -121.94 lon; 49.73 depth; utmZone: 10 2024-11-06T05:02:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869323.87 2024-11-06T05:02:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:04Z- waiting for 1730869327.87, time is 1730869324.72 2024-11-06T05:02:05Z- AHRS_M2 msg: 1730869325.07 epoch sec; seqNo:3736 2024-11-06T05:02:05Z- AHRS_M2 msg: 0.16 phi; 0.18 theta; 0.51 psi 2024-11-06T05:02:05Z- handleDepth()- Depth_Keller msg: 1730869325.07 epoch sec; seqNo:3979; depth 49.66 2024-11-06T05:02:05Z- handleDVL() - DVL msg epochMillisec: 1730869325.10 2024-11-06T05:02:05Z- handleDvl() - RDI_Pathfinder msg: 1730869325.10 sec; seqNo:1393; #beams:4 2024-11-06T05:02:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.67 , 39.55 , 47.51 , 38.13 2024-11-06T05:02:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.82 , -0.07 , 0.02 2024-11-06T05:02:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869325.11 epoch sec; seqNo:3979; 4061437.05 north; 594797.07 east; 36.69 lat; -121.94 lon; 49.66 depth; utmZone: 10 2024-11-06T05:02:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869323.87 2024-11-06T05:02:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:05Z- waiting for 1730869327.87, time is 1730869325.11 2024-11-06T05:02:05Z- AHRS_M2 msg: 1730869325.51 epoch sec; seqNo:3737 2024-11-06T05:02:05Z- AHRS_M2 msg: 0.16 phi; 0.18 theta; 0.51 psi 2024-11-06T05:02:05Z- handleDepth()- Depth_Keller msg: 1730869325.51 epoch sec; seqNo:3980; depth 49.59 2024-11-06T05:02:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869325.53 epoch sec; seqNo:3980; 4061437.44 north; 594798.29 east; 36.69 lat; -121.94 lon; 49.59 depth; utmZone: 10 2024-11-06T05:02:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869323.87 2024-11-06T05:02:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:05Z- waiting for 1730869327.87, time is 1730869325.53 2024-11-06T05:02:05Z- AHRS_M2 msg: 1730869325.91 epoch sec; seqNo:3738 2024-11-06T05:02:05Z- AHRS_M2 msg: 0.16 phi; 0.18 theta; 0.51 psi 2024-11-06T05:02:05Z- handleDepth()- Depth_Keller msg: 1730869325.91 epoch sec; seqNo:3981; depth 49.52 2024-11-06T05:02:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869325.93 epoch sec; seqNo:3981; 4061437.44 north; 594798.29 east; 36.69 lat; -121.94 lon; 49.52 depth; utmZone: 10 2024-11-06T05:02:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869323.87 2024-11-06T05:02:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:06Z- waiting for 1730869327.87, time is 1730869325.93 2024-11-06T05:02:06Z- AHRS_M2 msg: 1730869326.31 epoch sec; seqNo:3739 2024-11-06T05:02:06Z- AHRS_M2 msg: 0.16 phi; 0.17 theta; 0.52 psi 2024-11-06T05:02:06Z- handleDepth()- Depth_Keller msg: 1730869326.32 epoch sec; seqNo:3982; depth 49.43 2024-11-06T05:02:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869326.33 epoch sec; seqNo:3982; 4061437.82 north; 594798.29 east; 36.69 lat; -121.94 lon; 49.43 depth; utmZone: 10 2024-11-06T05:02:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869323.87 2024-11-06T05:02:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:06Z- waiting for 1730869327.87, time is 1730869326.33 2024-11-06T05:02:06Z- AHRS_M2 msg: 1730869326.75 epoch sec; seqNo:3740 2024-11-06T05:02:06Z- AHRS_M2 msg: 0.15 phi; 0.16 theta; 0.52 psi 2024-11-06T05:02:06Z- handleDepth()- Depth_Keller msg: 1730869326.76 epoch sec; seqNo:3983; depth 49.38 2024-11-06T05:02:06Z- handleDVL() - DVL msg epochMillisec: 1730869326.78 2024-11-06T05:02:06Z- handleDvl() - RDI_Pathfinder msg: 1730869326.78 sec; seqNo:1394; #beams:4 2024-11-06T05:02:06Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.88 , 39.96 , 47.30 , 38.53 2024-11-06T05:02:06Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.06 , 0.00 2024-11-06T05:02:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869326.84 epoch sec; seqNo:3983; 4061438.20 north; 594798.28 east; 36.69 lat; -121.94 lon; 49.38 depth; utmZone: 10 2024-11-06T05:02:06Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869323.87 2024-11-06T05:02:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:06Z- waiting for 1730869327.87, time is 1730869326.84 2024-11-06T05:02:07Z- handleDepth()- Depth_Keller msg: 1730869327.14 epoch sec; seqNo:3984; depth 49.29 2024-11-06T05:02:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869327.16 epoch sec; seqNo:3984; 4061438.58 north; 594798.28 east; 36.69 lat; -121.94 lon; 49.29 depth; utmZone: 10 2024-11-06T05:02:07Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:02:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869323.87 2024-11-06T05:02:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:07Z- waiting for 1730869327.87, time is 1730869327.16 2024-11-06T05:02:07Z- AHRS_M2 msg: 1730869327.52 epoch sec; seqNo:3741 2024-11-06T05:02:07Z- AHRS_M2 msg: 0.15 phi; 0.16 theta; 0.52 psi 2024-11-06T05:02:07Z- handleDepth()- Depth_Keller msg: 1730869327.53 epoch sec; seqNo:3985; depth 49.24 2024-11-06T05:02:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869327.54 epoch sec; seqNo:3985; 4061438.96 north; 594798.27 east; 36.69 lat; -121.94 lon; 49.24 depth; utmZone: 10 2024-11-06T05:02:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869323.87 2024-11-06T05:02:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:07Z- waiting for 1730869327.87, time is 1730869327.54 2024-11-06T05:02:07Z- AHRS_M2 msg: 1730869327.94 epoch sec; seqNo:3742 2024-11-06T05:02:07Z- AHRS_M2 msg: 0.15 phi; 0.15 theta; 0.52 psi 2024-11-06T05:02:07Z- handleDepth()- Depth_Keller msg: 1730869327.95 epoch sec; seqNo:3986; depth 49.15 2024-11-06T05:02:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869327.96 epoch sec; seqNo:3986; 4061438.96 north; 594798.27 east; 36.69 lat; -121.94 lon; 49.15 depth; utmZone: 10 2024-11-06T05:02:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869323.87 2024-11-06T05:02:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:08Z- waiting for 1730869327.87, time is 1730869327.96 2024-11-06T05:02:08Z- LcmTrn::updateTrn() >>>> pose time 1730869327.96, meas time 1730869326.78 <<<< 2024-11-06T05:02:08Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869327.96 2024-11-06T05:02:08Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869326.78 sec, ping # 0; waiting for INS data more recent than 1730869323.87... 2024-11-06T05:02:08Z- TerrainNav::motionUpdate() - currEstimate = (4061439, 594798.27, 49.15) 2024-11-06T05:02:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:02:08Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:02:08Z- TNavPF::Projecting Measurements 2024-11-06T05:02:08Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:02:08Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:02:08Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:02:08Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:02:08Z- TNavFilter::4 good beams. 2024-11-06T05:02:08Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:02:08Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:02:08Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:02:08Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:02:08Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:02:08Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:02:08Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:02:08Z- TerrainMapOctree: (vehN, vehE ) = (4061438.08, 594797.93). 2024-11-06T05:02:08Z- TerrainMapOctree: Min Distance = 194.09. 2024-11-06T05:02:08Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:02:08Z- Forcing Subcloud Comparison 2024-11-06T05:02:08Z- Weighting particles with subcloud comparison 2024-11-06T05:02:08Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:08Z- meanExpectedMeasDiff: -0.208071 delta_rms_squared: 1.477305 alpha: 0.986643 2024-11-06T05:02:08Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:08Z- meanExpectedMeasDiff: 0.230659 delta_rms_squared: 1.110780 alpha: 0.982313 2024-11-06T05:02:08Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:08Z- meanExpectedMeasDiff: -0.520672 delta_rms_squared: 1.613714 alpha: 0.987758 2024-11-06T05:02:08Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:08Z- meanExpectedMeasDiff: 0.156101 delta_rms_squared: 1.095475 alpha: 0.982070 2024-11-06T05:02:08Z- Subcloud NIS: 0.188915 2024-11-06T05:02:08Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:02:08Z- TNavPF:: sumWeights = 0.662548 2024-11-06T05:02:08Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:02:08Z- TNavPF::First Term of Map Covariance Matrix is 0.48 2024-11-06T05:02:08Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T05:02:08Z- TerrainNav::Resampling particle filter... 2024-11-06T05:02:08Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869326.78 sec. 2024-11-06T05:02:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:02:08Z- variances([0], [2], and psi_berg) = 0.803114 0.472931 0.000000 2024-11-06T05:02:08Z- publishEstimatesDecoder() - at 1730869327.96 sec 2024-11-06T05:02:08Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:02:08Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061384.91 594801.79 57.57 2024-11-06T05:02:08Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061382.19 594802.76 57.05 lat/lon: 36.69 -121.94 2024-11-06T05:02:08Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.80 0.47 0.69 0.00 2024-11-06T05:02:08Z- AHRS_M2 msg: 1730869328.34 epoch sec; seqNo:3743 2024-11-06T05:02:08Z- AHRS_M2 msg: 0.14 phi; 0.15 theta; 0.52 psi 2024-11-06T05:02:08Z- handleDepth()- Depth_Keller msg: 1730869328.36 epoch sec; seqNo:3987; depth 49.10 2024-11-06T05:02:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869328.37 epoch sec; seqNo:3987; 4061439.33 north; 594798.27 east; 36.69 lat; -121.94 lon; 49.10 depth; utmZone: 10 2024-11-06T05:02:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869327.96 2024-11-06T05:02:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:08Z- waiting for 1730869331.96, time is 1730869328.37 2024-11-06T05:02:08Z- handleDepth()- Depth_Keller msg: 1730869328.79 epoch sec; seqNo:3988; depth 49.03 2024-11-06T05:02:08Z- handleDVL() - DVL msg epochMillisec: 1730869328.83 2024-11-06T05:02:08Z- handleDvl() - RDI_Pathfinder msg: 1730869328.83 sec; seqNo:1395; #beams:4 2024-11-06T05:02:08Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.47 , 40.17 , 46.49 , 39.15 2024-11-06T05:02:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.07 , -0.02 2024-11-06T05:02:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869328.84 epoch sec; seqNo:3988; 4061439.73 north; 594799.49 east; 36.69 lat; -121.94 lon; 49.03 depth; utmZone: 10 2024-11-06T05:02:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869327.96 2024-11-06T05:02:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:08Z- waiting for 1730869331.96, time is 1730869328.84 2024-11-06T05:02:09Z- AHRS_M2 msg: 1730869329.17 epoch sec; seqNo:3744 2024-11-06T05:02:09Z- AHRS_M2 msg: 0.14 phi; 0.14 theta; 0.52 psi 2024-11-06T05:02:09Z- handleDepth()- Depth_Keller msg: 1730869329.17 epoch sec; seqNo:3989; depth 48.94 2024-11-06T05:02:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869329.20 epoch sec; seqNo:3989; 4061440.11 north; 594799.48 east; 36.69 lat; -121.94 lon; 48.94 depth; utmZone: 10 2024-11-06T05:02:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869327.96 2024-11-06T05:02:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:09Z- waiting for 1730869331.96, time is 1730869329.20 2024-11-06T05:02:09Z- AHRS_M2 msg: 1730869329.57 epoch sec; seqNo:3745 2024-11-06T05:02:09Z- AHRS_M2 msg: 0.14 phi; 0.13 theta; 0.52 psi 2024-11-06T05:02:09Z- handleDepth()- Depth_Keller msg: 1730869329.58 epoch sec; seqNo:3990; depth 48.89 2024-11-06T05:02:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869329.59 epoch sec; seqNo:3990; 4061440.48 north; 594799.48 east; 36.69 lat; -121.94 lon; 48.89 depth; utmZone: 10 2024-11-06T05:02:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869327.96 2024-11-06T05:02:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:09Z- waiting for 1730869331.96, time is 1730869329.59 2024-11-06T05:02:09Z- AHRS_M2 msg: 1730869329.97 epoch sec; seqNo:3746 2024-11-06T05:02:09Z- AHRS_M2 msg: 0.13 phi; 0.13 theta; 0.51 psi 2024-11-06T05:02:09Z- handleDepth()- Depth_Keller msg: 1730869329.98 epoch sec; seqNo:3991; depth 48.84 2024-11-06T05:02:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869329.99 epoch sec; seqNo:3991; 4061440.48 north; 594799.48 east; 36.69 lat; -121.94 lon; 48.84 depth; utmZone: 10 2024-11-06T05:02:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869327.96 2024-11-06T05:02:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:10Z- waiting for 1730869331.96, time is 1730869329.99 2024-11-06T05:02:10Z- AHRS_M2 msg: 1730869330.38 epoch sec; seqNo:3747 2024-11-06T05:02:10Z- AHRS_M2 msg: 0.13 phi; 0.12 theta; 0.51 psi 2024-11-06T05:02:10Z- handleDepth()- Depth_Keller msg: 1730869330.39 epoch sec; seqNo:3992; depth 48.79 2024-11-06T05:02:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869330.40 epoch sec; seqNo:3992; 4061440.86 north; 594799.47 east; 36.69 lat; -121.94 lon; 48.79 depth; utmZone: 10 2024-11-06T05:02:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869327.96 2024-11-06T05:02:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:10Z- waiting for 1730869331.96, time is 1730869330.40 2024-11-06T05:02:10Z- handleDepth()- Depth_Keller msg: 1730869330.81 epoch sec; seqNo:3993; depth 48.72 2024-11-06T05:02:10Z- handleDVL() - DVL msg epochMillisec: 1730869330.84 2024-11-06T05:02:10Z- handleDvl() - RDI_Pathfinder msg: 1730869330.84 sec; seqNo:1396; #beams:4 2024-11-06T05:02:10Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.26 , 40.57 , 45.67 , 39.76 2024-11-06T05:02:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.10 , -0.03 2024-11-06T05:02:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869330.86 epoch sec; seqNo:3993; 4061441.24 north; 594799.47 east; 36.69 lat; -121.94 lon; 48.72 depth; utmZone: 10 2024-11-06T05:02:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869327.96 2024-11-06T05:02:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:10Z- waiting for 1730869331.96, time is 1730869330.86 2024-11-06T05:02:11Z- AHRS_M2 msg: 1730869331.19 epoch sec; seqNo:3748 2024-11-06T05:02:11Z- AHRS_M2 msg: 0.13 phi; 0.11 theta; 0.51 psi 2024-11-06T05:02:11Z- handleDepth()- Depth_Keller msg: 1730869331.19 epoch sec; seqNo:3994; depth 48.66 2024-11-06T05:02:11Z- handleDVL() - DVL msg epochMillisec: 1730869331.20 2024-11-06T05:02:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:02:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:02:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:02:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:02:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:02:11Z- handleDvl() - RDI_Pathfinder msg: 1730869331.20 sec; seqNo:1397; #beams:0 2024-11-06T05:02:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:02:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.83 , -0.10 , -0.03 2024-11-06T05:02:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869331.21 epoch sec; seqNo:3994; 4061441.62 north; 594799.46 east; 36.69 lat; -121.94 lon; 48.66 depth; utmZone: 10 2024-11-06T05:02:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869327.96 2024-11-06T05:02:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:11Z- waiting for 1730869331.96, time is 1730869331.21 2024-11-06T05:02:11Z- AHRS_M2 msg: 1730869331.59 epoch sec; seqNo:3749 2024-11-06T05:02:11Z- AHRS_M2 msg: 0.13 phi; 0.10 theta; 0.51 psi 2024-11-06T05:02:11Z- handleDepth()- Depth_Keller msg: 1730869331.60 epoch sec; seqNo:3995; depth 48.62 2024-11-06T05:02:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869331.61 epoch sec; seqNo:3995; 4061442.00 north; 594799.46 east; 36.69 lat; -121.94 lon; 48.62 depth; utmZone: 10 2024-11-06T05:02:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869327.96 2024-11-06T05:02:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:11Z- waiting for 1730869331.96, time is 1730869331.61 2024-11-06T05:02:12Z- AHRS_M2 msg: 1730869332.01 epoch sec; seqNo:3750 2024-11-06T05:02:12Z- AHRS_M2 msg: 0.13 phi; 0.09 theta; 0.51 psi 2024-11-06T05:02:12Z- handleDepth()- Depth_Keller msg: 1730869332.01 epoch sec; seqNo:3996; depth 48.57 2024-11-06T05:02:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869332.02 epoch sec; seqNo:3996; 4061442.00 north; 594799.46 east; 36.69 lat; -121.94 lon; 48.57 depth; utmZone: 10 2024-11-06T05:02:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869327.96 2024-11-06T05:02:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:12Z- waiting for 1730869331.96, time is 1730869332.02 2024-11-06T05:02:12Z- LcmTrn::updateTrn() >>>> pose time 1730869332.02, meas time 1730869331.20 <<<< 2024-11-06T05:02:12Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869332.02 2024-11-06T05:02:12Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T05:02:12Z- TerrainNav::motionUpdate() - currEstimate = (4061442, 594799.46, 48.57) 2024-11-06T05:02:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:02:12Z- TerrainNav:: SubcloudNIS is 0.19, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:02:12Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T05:02:12Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T05:02:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:02:12Z- variances([0], [2], and psi_berg) = 0.804914 0.474853 0.000000 2024-11-06T05:02:12Z- publishEstimatesDecoder() - at 1730869332.02 sec 2024-11-06T05:02:12Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:02:12Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061387.93 594802.99 56.73 2024-11-06T05:02:12Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061385.23 594803.94 56.46 lat/lon: 36.69 -121.94 2024-11-06T05:02:12Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.80 0.47 0.93 0.00 2024-11-06T05:02:12Z- AHRS_M2 msg: 1730869332.40 epoch sec; seqNo:3751 2024-11-06T05:02:12Z- AHRS_M2 msg: 0.13 phi; 0.09 theta; 0.51 psi 2024-11-06T05:02:12Z- handleDepth()- Depth_Keller msg: 1730869332.41 epoch sec; seqNo:3997; depth 48.50 2024-11-06T05:02:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869332.42 epoch sec; seqNo:3997; 4061442.39 north; 594800.68 east; 36.69 lat; -121.94 lon; 48.50 depth; utmZone: 10 2024-11-06T05:02:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869332.02 2024-11-06T05:02:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:12Z- waiting for 1730869336.02, time is 1730869332.42 2024-11-06T05:02:12Z- AHRS_M2 msg: 1730869332.81 epoch sec; seqNo:3752 2024-11-06T05:02:12Z- AHRS_M2 msg: 0.12 phi; 0.08 theta; 0.50 psi 2024-11-06T05:02:12Z- handleDepth()- Depth_Keller msg: 1730869332.82 epoch sec; seqNo:3998; depth 48.46 2024-11-06T05:02:12Z- handleDVL() - DVL msg epochMillisec: 1730869332.85 2024-11-06T05:02:12Z- handleDvl() - RDI_Pathfinder msg: 1730869332.85 sec; seqNo:1398; #beams:4 2024-11-06T05:02:12Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 45.06 , 40.57 , 44.65 , 40.37 2024-11-06T05:02:12Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.12 , 0.00 2024-11-06T05:02:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869332.87 epoch sec; seqNo:3998; 4061442.77 north; 594800.67 east; 36.69 lat; -121.94 lon; 48.46 depth; utmZone: 10 2024-11-06T05:02:12Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869332.02 2024-11-06T05:02:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:12Z- waiting for 1730869336.02, time is 1730869332.87 2024-11-06T05:02:13Z- AHRS_M2 msg: 1730869333.21 epoch sec; seqNo:3753 2024-11-06T05:02:13Z- AHRS_M2 msg: 0.12 phi; 0.08 theta; 0.50 psi 2024-11-06T05:02:13Z- handleDepth()- Depth_Keller msg: 1730869333.21 epoch sec; seqNo:3999; depth 48.42 2024-11-06T05:02:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869333.23 epoch sec; seqNo:3999; 4061443.15 north; 594800.67 east; 36.69 lat; -121.94 lon; 48.42 depth; utmZone: 10 2024-11-06T05:02:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869332.02 2024-11-06T05:02:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:13Z- waiting for 1730869336.02, time is 1730869333.23 2024-11-06T05:02:13Z- AHRS_M2 msg: 1730869333.62 epoch sec; seqNo:3754 2024-11-06T05:02:13Z- AHRS_M2 msg: 0.11 phi; 0.07 theta; 0.49 psi 2024-11-06T05:02:13Z- handleDepth()- Depth_Keller msg: 1730869333.63 epoch sec; seqNo:4000; depth 48.39 2024-11-06T05:02:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869333.63 epoch sec; seqNo:4000; 4061443.53 north; 594800.66 east; 36.69 lat; -121.94 lon; 48.39 depth; utmZone: 10 2024-11-06T05:02:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869332.02 2024-11-06T05:02:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:13Z- waiting for 1730869336.02, time is 1730869333.63 2024-11-06T05:02:14Z- AHRS_M2 msg: 1730869334.02 epoch sec; seqNo:3755 2024-11-06T05:02:14Z- AHRS_M2 msg: 0.10 phi; 0.07 theta; 0.49 psi 2024-11-06T05:02:14Z- handleDepth()- Depth_Keller msg: 1730869334.03 epoch sec; seqNo:4001; depth 48.34 2024-11-06T05:02:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869334.05 epoch sec; seqNo:4001; 4061443.91 north; 594800.66 east; 36.69 lat; -121.94 lon; 48.34 depth; utmZone: 10 2024-11-06T05:02:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869332.02 2024-11-06T05:02:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:14Z- waiting for 1730869336.02, time is 1730869334.05 2024-11-06T05:02:14Z- AHRS_M2 msg: 1730869334.42 epoch sec; seqNo:3756 2024-11-06T05:02:14Z- AHRS_M2 msg: 0.10 phi; 0.06 theta; 0.48 psi 2024-11-06T05:02:14Z- handleDepth()- Depth_Keller msg: 1730869334.42 epoch sec; seqNo:4002; depth 48.30 2024-11-06T05:02:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869334.43 epoch sec; seqNo:4002; 4061444.29 north; 594800.66 east; 36.69 lat; -121.94 lon; 48.30 depth; utmZone: 10 2024-11-06T05:02:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869332.02 2024-11-06T05:02:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:14Z- waiting for 1730869336.02, time is 1730869334.43 2024-11-06T05:02:14Z- AHRS_M2 msg: 1730869334.83 epoch sec; seqNo:3757 2024-11-06T05:02:14Z- AHRS_M2 msg: 0.09 phi; 0.06 theta; 0.48 psi 2024-11-06T05:02:14Z- handleDepth()- Depth_Keller msg: 1730869334.84 epoch sec; seqNo:4003; depth 48.27 2024-11-06T05:02:14Z- handleDVL() - DVL msg epochMillisec: 1730869334.86 2024-11-06T05:02:14Z- handleDvl() - RDI_Pathfinder msg: 1730869334.86 sec; seqNo:1399; #beams:4 2024-11-06T05:02:14Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.45 , 41.19 , 44.04 , 40.98 2024-11-06T05:02:14Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.09 , 0.04 2024-11-06T05:02:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869334.90 epoch sec; seqNo:4003; 4061444.67 north; 594800.65 east; 36.69 lat; -121.94 lon; 48.27 depth; utmZone: 10 2024-11-06T05:02:14Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869332.02 2024-11-06T05:02:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:14Z- waiting for 1730869336.02, time is 1730869334.90 2024-11-06T05:02:15Z- AHRS_M2 msg: 1730869335.23 epoch sec; seqNo:3758 2024-11-06T05:02:15Z- AHRS_M2 msg: 0.09 phi; 0.05 theta; 0.47 psi 2024-11-06T05:02:15Z- handleDepth()- Depth_Keller msg: 1730869335.23 epoch sec; seqNo:4004; depth 48.23 2024-11-06T05:02:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869335.26 epoch sec; seqNo:4004; 4061444.67 north; 594800.65 east; 36.69 lat; -121.94 lon; 48.23 depth; utmZone: 10 2024-11-06T05:02:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869332.02 2024-11-06T05:02:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:15Z- waiting for 1730869336.02, time is 1730869335.26 2024-11-06T05:02:15Z- AHRS_M2 msg: 1730869335.64 epoch sec; seqNo:3759 2024-11-06T05:02:15Z- AHRS_M2 msg: 0.09 phi; 0.05 theta; 0.47 psi 2024-11-06T05:02:15Z- handleDepth()- Depth_Keller msg: 1730869335.65 epoch sec; seqNo:4005; depth 48.20 2024-11-06T05:02:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869335.66 epoch sec; seqNo:4005; 4061445.04 north; 594800.65 east; 36.69 lat; -121.94 lon; 48.20 depth; utmZone: 10 2024-11-06T05:02:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869332.02 2024-11-06T05:02:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:15Z- waiting for 1730869336.02, time is 1730869335.66 2024-11-06T05:02:16Z- AHRS_M2 msg: 1730869336.04 epoch sec; seqNo:3760 2024-11-06T05:02:16Z- AHRS_M2 msg: 0.09 phi; 0.04 theta; 0.47 psi 2024-11-06T05:02:16Z- handleDepth()- Depth_Keller msg: 1730869336.05 epoch sec; seqNo:4006; depth 48.17 2024-11-06T05:02:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869336.06 epoch sec; seqNo:4006; 4061445.42 north; 594800.64 east; 36.69 lat; -121.94 lon; 48.17 depth; utmZone: 10 2024-11-06T05:02:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869332.02 2024-11-06T05:02:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:16Z- waiting for 1730869336.02, time is 1730869336.06 2024-11-06T05:02:16Z- LcmTrn::updateTrn() >>>> pose time 1730869336.06, meas time 1730869334.86 <<<< 2024-11-06T05:02:16Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869336.06 2024-11-06T05:02:16Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869334.86 sec, ping # 0; waiting for INS data more recent than 1730869332.02... 2024-11-06T05:02:16Z- TerrainNav::motionUpdate() - currEstimate = (4061445, 594800.64, 48.17) 2024-11-06T05:02:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:02:16Z- TerrainNav:: SubcloudNIS is 0.19, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:02:16Z- TNavPF::Projecting Measurements 2024-11-06T05:02:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:02:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:02:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:02:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:02:16Z- TNavFilter::4 good beams. 2024-11-06T05:02:16Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:02:16Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:02:16Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:02:16Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:02:16Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:02:16Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:02:16Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:02:16Z- TerrainMapOctree: (vehN, vehE ) = (4061444.41, 594800.29). 2024-11-06T05:02:16Z- TerrainMapOctree: Min Distance = 200.81. 2024-11-06T05:02:16Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:02:16Z- Forcing Subcloud Comparison 2024-11-06T05:02:16Z- Weighting particles with subcloud comparison 2024-11-06T05:02:16Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:16Z- meanExpectedMeasDiff: -0.649374 delta_rms_squared: 1.788476 alpha: 0.988941 2024-11-06T05:02:16Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:16Z- meanExpectedMeasDiff: -0.104252 delta_rms_squared: 1.418781 alpha: 0.986099 2024-11-06T05:02:16Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:16Z- meanExpectedMeasDiff: -1.182287 delta_rms_squared: 1.703944 alpha: 0.988399 2024-11-06T05:02:16Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:16Z- meanExpectedMeasDiff: -0.105836 delta_rms_squared: 1.483257 alpha: 0.986696 2024-11-06T05:02:16Z- Subcloud NIS: 0.186820 2024-11-06T05:02:16Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:02:16Z- TNavPF:: sumWeights = 0.577008 2024-11-06T05:02:16Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:02:16Z- TNavPF::First Term of Map Covariance Matrix is 0.44 2024-11-06T05:02:16Z- TNavPF::Calculated NIS Value : 0.09 numBeams normalized NIS: 0.02 2024-11-06T05:02:16Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869334.86 sec. 2024-11-06T05:02:16Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:02:16Z- variances([0], [2], and psi_berg) = 0.808810 0.480890 0.000000 2024-11-06T05:02:16Z- publishEstimatesDecoder() - at 1730869336.06 sec 2024-11-06T05:02:16Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:02:16Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061389.59 594804.72 55.90 2024-11-06T05:02:16Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061388.65 594805.12 56.31 lat/lon: 36.69 -121.94 2024-11-06T05:02:16Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.81 0.48 0.74 0.00 2024-11-06T05:02:16Z- AHRS_M2 msg: 1730869336.47 epoch sec; seqNo:3761 2024-11-06T05:02:16Z- AHRS_M2 msg: 0.08 phi; 0.04 theta; 0.46 psi 2024-11-06T05:02:16Z- handleDepth()- Depth_Keller msg: 1730869336.51 epoch sec; seqNo:4007; depth 48.14 2024-11-06T05:02:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869336.52 epoch sec; seqNo:4007; 4061445.82 north; 594801.86 east; 36.69 lat; -121.94 lon; 48.14 depth; utmZone: 10 2024-11-06T05:02:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869336.06 2024-11-06T05:02:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:16Z- waiting for 1730869340.06, time is 1730869336.52 2024-11-06T05:02:16Z- AHRS_M2 msg: 1730869336.84 epoch sec; seqNo:3762 2024-11-06T05:02:16Z- AHRS_M2 msg: 0.08 phi; 0.04 theta; 0.46 psi 2024-11-06T05:02:16Z- handleDepth()- Depth_Keller msg: 1730869336.85 epoch sec; seqNo:4008; depth 48.11 2024-11-06T05:02:16Z- handleDVL() - DVL msg epochMillisec: 1730869336.87 2024-11-06T05:02:16Z- handleDvl() - RDI_Pathfinder msg: 1730869336.87 sec; seqNo:1400; #beams:4 2024-11-06T05:02:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.84 , 41.80 , 43.63 , 41.59 2024-11-06T05:02:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , -0.04 , 0.05 2024-11-06T05:02:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869336.90 epoch sec; seqNo:4008; 4061446.19 north; 594801.86 east; 36.69 lat; -121.94 lon; 48.11 depth; utmZone: 10 2024-11-06T05:02:17Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869336.06 2024-11-06T05:02:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:17Z- waiting for 1730869340.06, time is 1730869336.90 2024-11-06T05:02:17Z- AHRS_M2 msg: 1730869337.25 epoch sec; seqNo:3763 2024-11-06T05:02:17Z- AHRS_M2 msg: 0.07 phi; 0.03 theta; 0.45 psi 2024-11-06T05:02:17Z- handleDepth()- Depth_Keller msg: 1730869337.25 epoch sec; seqNo:4009; depth 48.09 2024-11-06T05:02:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869337.27 epoch sec; seqNo:4009; 4061446.19 north; 594801.86 east; 36.69 lat; -121.94 lon; 48.09 depth; utmZone: 10 2024-11-06T05:02:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869336.06 2024-11-06T05:02:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:17Z- waiting for 1730869340.06, time is 1730869337.27 2024-11-06T05:02:17Z- AHRS_M2 msg: 1730869337.65 epoch sec; seqNo:3764 2024-11-06T05:02:17Z- AHRS_M2 msg: 0.07 phi; 0.03 theta; 0.44 psi 2024-11-06T05:02:17Z- handleDepth()- Depth_Keller msg: 1730869337.66 epoch sec; seqNo:4010; depth 48.06 2024-11-06T05:02:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869337.68 epoch sec; seqNo:4010; 4061446.57 north; 594801.85 east; 36.69 lat; -121.94 lon; 48.06 depth; utmZone: 10 2024-11-06T05:02:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869336.06 2024-11-06T05:02:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:17Z- waiting for 1730869340.06, time is 1730869337.68 2024-11-06T05:02:18Z- AHRS_M2 msg: 1730869338.06 epoch sec; seqNo:3765 2024-11-06T05:02:18Z- AHRS_M2 msg: 0.06 phi; 0.03 theta; 0.44 psi 2024-11-06T05:02:18Z- handleDepth()- Depth_Keller msg: 1730869338.06 epoch sec; seqNo:4011; depth 48.03 2024-11-06T05:02:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869338.07 epoch sec; seqNo:4011; 4061446.95 north; 594801.85 east; 36.69 lat; -121.94 lon; 48.03 depth; utmZone: 10 2024-11-06T05:02:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869336.06 2024-11-06T05:02:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:18Z- waiting for 1730869340.06, time is 1730869338.07 2024-11-06T05:02:18Z- handleDepth()- Depth_Keller msg: 1730869338.48 epoch sec; seqNo:4012; depth 48.01 2024-11-06T05:02:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869338.50 epoch sec; seqNo:4012; 4061447.33 north; 594801.84 east; 36.69 lat; -121.94 lon; 48.01 depth; utmZone: 10 2024-11-06T05:02:18Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:02:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869336.06 2024-11-06T05:02:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:18Z- waiting for 1730869340.06, time is 1730869338.50 2024-11-06T05:02:18Z- AHRS_M2 msg: 1730869338.86 epoch sec; seqNo:3766 2024-11-06T05:02:18Z- AHRS_M2 msg: 0.05 phi; 0.02 theta; 0.42 psi 2024-11-06T05:02:18Z- handleDepth()- Depth_Keller msg: 1730869338.87 epoch sec; seqNo:4013; depth 47.97 2024-11-06T05:02:18Z- handleDVL() - DVL msg epochMillisec: 1730869338.89 2024-11-06T05:02:18Z- handleDvl() - RDI_Pathfinder msg: 1730869338.89 sec; seqNo:1401; #beams:4 2024-11-06T05:02:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.22 , 42.41 , 43.22 , 42.00 2024-11-06T05:02:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , 0.01 2024-11-06T05:02:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869338.92 epoch sec; seqNo:4013; 4061447.71 north; 594801.84 east; 36.69 lat; -121.94 lon; 47.97 depth; utmZone: 10 2024-11-06T05:02:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869336.06 2024-11-06T05:02:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:19Z- waiting for 1730869340.06, time is 1730869338.92 2024-11-06T05:02:19Z- AHRS_M2 msg: 1730869339.27 epoch sec; seqNo:3767 2024-11-06T05:02:19Z- AHRS_M2 msg: 0.05 phi; 0.02 theta; 0.42 psi 2024-11-06T05:02:19Z- handleDepth()- Depth_Keller msg: 1730869339.28 epoch sec; seqNo:4014; depth 47.96 2024-11-06T05:02:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869339.30 epoch sec; seqNo:4014; 4061448.09 north; 594801.83 east; 36.69 lat; -121.94 lon; 47.96 depth; utmZone: 10 2024-11-06T05:02:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869336.06 2024-11-06T05:02:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:19Z- waiting for 1730869340.06, time is 1730869339.30 2024-11-06T05:02:19Z- AHRS_M2 msg: 1730869339.68 epoch sec; seqNo:3768 2024-11-06T05:02:19Z- AHRS_M2 msg: 0.05 phi; 0.01 theta; 0.41 psi 2024-11-06T05:02:19Z- handleDepth()- Depth_Keller msg: 1730869339.68 epoch sec; seqNo:4015; depth 47.94 2024-11-06T05:02:19Z- handleDVL() - DVL msg epochMillisec: 1730869339.68 2024-11-06T05:02:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:02:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:02:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:02:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:02:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:02:19Z- handleDvl() - RDI_Pathfinder msg: 1730869339.68 sec; seqNo:1402; #beams:0 2024-11-06T05:02:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:02:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.03 , 0.01 2024-11-06T05:02:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869339.69 epoch sec; seqNo:4015; 4061448.09 north; 594801.83 east; 36.69 lat; -121.94 lon; 47.94 depth; utmZone: 10 2024-11-06T05:02:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869336.06 2024-11-06T05:02:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:19Z- waiting for 1730869340.06, time is 1730869339.69 2024-11-06T05:02:20Z- AHRS_M2 msg: 1730869340.08 epoch sec; seqNo:3769 2024-11-06T05:02:20Z- AHRS_M2 msg: 0.05 phi; 0.01 theta; 0.40 psi 2024-11-06T05:02:20Z- handleDepth()- Depth_Keller msg: 1730869340.09 epoch sec; seqNo:4016; depth 47.92 2024-11-06T05:02:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869340.10 epoch sec; seqNo:4016; 4061448.47 north; 594801.83 east; 36.69 lat; -121.94 lon; 47.92 depth; utmZone: 10 2024-11-06T05:02:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869336.06 2024-11-06T05:02:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:20Z- waiting for 1730869340.06, time is 1730869340.10 2024-11-06T05:02:20Z- LcmTrn::updateTrn() >>>> pose time 1730869340.10, meas time 1730869339.68 <<<< 2024-11-06T05:02:20Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869340.10 2024-11-06T05:02:20Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T05:02:20Z- TerrainNav::motionUpdate() - currEstimate = (4061448, 594801.83, 47.92) 2024-11-06T05:02:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:02:20Z- TerrainNav:: SubcloudNIS is 0.19, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:02:20Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T05:02:20Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T05:02:20Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:02:20Z- variances([0], [2], and psi_berg) = 0.810864 0.482285 0.000000 2024-11-06T05:02:20Z- publishEstimatesDecoder() - at 1730869340.10 sec 2024-11-06T05:02:20Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:02:20Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061392.65 594805.93 55.59 2024-11-06T05:02:20Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061391.70 594806.31 56.06 lat/lon: 36.69 -121.94 2024-11-06T05:02:20Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.81 0.48 0.97 0.00 2024-11-06T05:02:20Z- AHRS_M2 msg: 1730869340.48 epoch sec; seqNo:3770 2024-11-06T05:02:20Z- AHRS_M2 msg: 0.05 phi; 0.01 theta; 0.40 psi 2024-11-06T05:02:20Z- handleDepth()- Depth_Keller msg: 1730869340.49 epoch sec; seqNo:4017; depth 47.89 2024-11-06T05:02:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869340.50 epoch sec; seqNo:4017; 4061448.86 north; 594803.05 east; 36.69 lat; -121.94 lon; 47.89 depth; utmZone: 10 2024-11-06T05:02:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869340.10 2024-11-06T05:02:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:20Z- waiting for 1730869344.10, time is 1730869340.50 2024-11-06T05:02:20Z- AHRS_M2 msg: 1730869340.88 epoch sec; seqNo:3771 2024-11-06T05:02:20Z- AHRS_M2 msg: 0.05 phi; 0.01 theta; 0.39 psi 2024-11-06T05:02:20Z- handleDepth()- Depth_Keller msg: 1730869340.89 epoch sec; seqNo:4018; depth 47.86 2024-11-06T05:02:20Z- handleDVL() - DVL msg epochMillisec: 1730869340.91 2024-11-06T05:02:20Z- handleDvl() - RDI_Pathfinder msg: 1730869340.91 sec; seqNo:1403; #beams:4 2024-11-06T05:02:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.82 , 42.82 , 42.82 , 42.41 2024-11-06T05:02:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.07 , -0.02 2024-11-06T05:02:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869340.93 epoch sec; seqNo:4018; 4061449.24 north; 594803.04 east; 36.69 lat; -121.94 lon; 47.86 depth; utmZone: 10 2024-11-06T05:02:21Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869340.10 2024-11-06T05:02:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:21Z- waiting for 1730869344.10, time is 1730869340.93 2024-11-06T05:02:21Z- AHRS_M2 msg: 1730869341.29 epoch sec; seqNo:3772 2024-11-06T05:02:21Z- AHRS_M2 msg: 0.05 phi; 0.00 theta; 0.38 psi 2024-11-06T05:02:21Z- handleDepth()- Depth_Keller msg: 1730869341.29 epoch sec; seqNo:4019; depth 47.85 2024-11-06T05:02:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869341.31 epoch sec; seqNo:4019; 4061449.62 north; 594803.04 east; 36.69 lat; -121.94 lon; 47.85 depth; utmZone: 10 2024-11-06T05:02:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869340.10 2024-11-06T05:02:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:21Z- waiting for 1730869344.10, time is 1730869341.31 2024-11-06T05:02:21Z- AHRS_M2 msg: 1730869341.70 epoch sec; seqNo:3773 2024-11-06T05:02:21Z- AHRS_M2 msg: 0.04 phi; 0.00 theta; 0.37 psi 2024-11-06T05:02:21Z- handleDepth()- Depth_Keller msg: 1730869341.70 epoch sec; seqNo:4020; depth 47.83 2024-11-06T05:02:21Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869341.71 epoch sec; seqNo:4020; 4061450.00 north; 594803.03 east; 36.69 lat; -121.94 lon; 47.83 depth; utmZone: 10 2024-11-06T05:02:21Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:21Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869340.10 2024-11-06T05:02:21Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:21Z- waiting for 1730869344.10, time is 1730869341.71 2024-11-06T05:02:22Z- AHRS_M2 msg: 1730869342.10 epoch sec; seqNo:3774 2024-11-06T05:02:22Z- AHRS_M2 msg: 0.04 phi; -0.00 theta; 0.36 psi 2024-11-06T05:02:22Z- handleDepth()- Depth_Keller msg: 1730869342.11 epoch sec; seqNo:4021; depth 47.81 2024-11-06T05:02:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869342.12 epoch sec; seqNo:4021; 4061450.38 north; 594803.03 east; 36.69 lat; -121.94 lon; 47.81 depth; utmZone: 10 2024-11-06T05:02:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869340.10 2024-11-06T05:02:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:22Z- waiting for 1730869344.10, time is 1730869342.12 2024-11-06T05:02:22Z- AHRS_M2 msg: 1730869342.49 epoch sec; seqNo:3775 2024-11-06T05:02:22Z- AHRS_M2 msg: 0.03 phi; -0.00 theta; 0.35 psi 2024-11-06T05:02:22Z- handleDepth()- Depth_Keller msg: 1730869342.50 epoch sec; seqNo:4022; depth 47.80 2024-11-06T05:02:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869342.51 epoch sec; seqNo:4022; 4061450.75 north; 594803.03 east; 36.69 lat; -121.94 lon; 47.80 depth; utmZone: 10 2024-11-06T05:02:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869340.10 2024-11-06T05:02:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:22Z- waiting for 1730869344.10, time is 1730869342.51 2024-11-06T05:02:22Z- AHRS_M2 msg: 1730869342.90 epoch sec; seqNo:3776 2024-11-06T05:02:22Z- AHRS_M2 msg: 0.03 phi; -0.01 theta; 0.34 psi 2024-11-06T05:02:22Z- handleDepth()- Depth_Keller msg: 1730869342.91 epoch sec; seqNo:4023; depth 47.79 2024-11-06T05:02:22Z- handleDVL() - DVL msg epochMillisec: 1730869342.93 2024-11-06T05:02:22Z- handleDvl() - RDI_Pathfinder msg: 1730869342.93 sec; seqNo:1404; #beams:4 2024-11-06T05:02:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.61 , 42.82 , 42.41 , 42.61 2024-11-06T05:02:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.13 , -0.02 2024-11-06T05:02:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869342.98 epoch sec; seqNo:4023; 4061451.13 north; 594803.02 east; 36.69 lat; -121.94 lon; 47.79 depth; utmZone: 10 2024-11-06T05:02:23Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869340.10 2024-11-06T05:02:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:23Z- waiting for 1730869344.10, time is 1730869342.98 2024-11-06T05:02:23Z- AHRS_M2 msg: 1730869343.33 epoch sec; seqNo:3777 2024-11-06T05:02:23Z- AHRS_M2 msg: 0.02 phi; -0.01 theta; 0.32 psi 2024-11-06T05:02:23Z- handleDepth()- Depth_Keller msg: 1730869343.33 epoch sec; seqNo:4024; depth 47.79 2024-11-06T05:02:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869343.35 epoch sec; seqNo:4024; 4061451.51 north; 594803.02 east; 36.69 lat; -121.94 lon; 47.79 depth; utmZone: 10 2024-11-06T05:02:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869340.10 2024-11-06T05:02:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:23Z- waiting for 1730869344.10, time is 1730869343.35 2024-11-06T05:02:23Z- AHRS_M2 msg: 1730869343.73 epoch sec; seqNo:3778 2024-11-06T05:02:23Z- AHRS_M2 msg: 0.02 phi; -0.01 theta; 0.30 psi 2024-11-06T05:02:23Z- handleDepth()- Depth_Keller msg: 1730869343.73 epoch sec; seqNo:4025; depth 47.77 2024-11-06T05:02:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869343.75 epoch sec; seqNo:4025; 4061451.51 north; 594803.02 east; 36.69 lat; -121.94 lon; 47.77 depth; utmZone: 10 2024-11-06T05:02:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869340.10 2024-11-06T05:02:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:23Z- waiting for 1730869344.10, time is 1730869343.75 2024-11-06T05:02:24Z- AHRS_M2 msg: 1730869344.13 epoch sec; seqNo:3779 2024-11-06T05:02:24Z- AHRS_M2 msg: 0.02 phi; -0.02 theta; 0.29 psi 2024-11-06T05:02:24Z- handleDepth()- Depth_Keller msg: 1730869344.14 epoch sec; seqNo:4026; depth 47.76 2024-11-06T05:02:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869344.15 epoch sec; seqNo:4026; 4061451.89 north; 594803.01 east; 36.69 lat; -121.94 lon; 47.76 depth; utmZone: 10 2024-11-06T05:02:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869340.10 2024-11-06T05:02:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:24Z- waiting for 1730869344.10, time is 1730869344.15 2024-11-06T05:02:24Z- LcmTrn::updateTrn() >>>> pose time 1730869344.15, meas time 1730869342.93 <<<< 2024-11-06T05:02:24Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869344.15 2024-11-06T05:02:24Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869342.93 sec, ping # 0; waiting for INS data more recent than 1730869340.10... 2024-11-06T05:02:24Z- TerrainNav::motionUpdate() - currEstimate = (4061452, 594803.01, 47.76) 2024-11-06T05:02:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:02:24Z- TerrainNav:: SubcloudNIS is 0.19, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:02:24Z- TNavPF::Projecting Measurements 2024-11-06T05:02:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:02:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:02:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:02:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:02:24Z- TNavFilter::4 good beams. 2024-11-06T05:02:24Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:02:24Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:02:24Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:02:24Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:02:24Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:02:24Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:02:24Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:02:24Z- TerrainMapOctree: (vehN, vehE ) = (4061450.86, 594802.66). 2024-11-06T05:02:24Z- TerrainMapOctree: Min Distance = 207.65. 2024-11-06T05:02:24Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:02:24Z- Forcing Subcloud Comparison 2024-11-06T05:02:24Z- Weighting particles with subcloud comparison 2024-11-06T05:02:24Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:24Z- meanExpectedMeasDiff: -0.542153 delta_rms_squared: 1.665843 alpha: 0.988137 2024-11-06T05:02:24Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:24Z- meanExpectedMeasDiff: -0.194795 delta_rms_squared: 1.656210 alpha: 0.988068 2024-11-06T05:02:24Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:24Z- meanExpectedMeasDiff: -1.003539 delta_rms_squared: 1.592187 alpha: 0.987594 2024-11-06T05:02:24Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:24Z- meanExpectedMeasDiff: -0.108939 delta_rms_squared: 1.632366 alpha: 0.987896 2024-11-06T05:02:24Z- Subcloud NIS: 0.182211 2024-11-06T05:02:24Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:02:24Z- TNavPF:: sumWeights = 0.588920 2024-11-06T05:02:24Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:02:24Z- TNavPF::First Term of Map Covariance Matrix is 0.42 2024-11-06T05:02:24Z- TNavPF::Calculated NIS Value : 0.07 numBeams normalized NIS: 0.02 2024-11-06T05:02:24Z- TerrainNav::Resampling particle filter... 2024-11-06T05:02:24Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869342.93 sec. 2024-11-06T05:02:24Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:02:24Z- variances([0], [2], and psi_berg) = 0.823163 0.490862 0.000000 2024-11-06T05:02:24Z- publishEstimatesDecoder() - at 1730869344.15 sec 2024-11-06T05:02:24Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:02:24Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061397.91 594806.47 55.31 2024-11-06T05:02:24Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061395.12 594807.49 56.14 lat/lon: 36.69 -121.94 2024-11-06T05:02:24Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.82 0.49 0.76 0.00 2024-11-06T05:02:24Z- AHRS_M2 msg: 1730869344.55 epoch sec; seqNo:3780 2024-11-06T05:02:24Z- AHRS_M2 msg: 0.02 phi; -0.02 theta; 0.28 psi 2024-11-06T05:02:24Z- handleDepth()- Depth_Keller msg: 1730869344.57 epoch sec; seqNo:4027; depth 47.76 2024-11-06T05:02:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869344.58 epoch sec; seqNo:4027; 4061452.27 north; 594803.01 east; 36.69 lat; -121.94 lon; 47.76 depth; utmZone: 10 2024-11-06T05:02:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869344.15 2024-11-06T05:02:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:24Z- waiting for 1730869348.15, time is 1730869344.58 2024-11-06T05:02:24Z- AHRS_M2 msg: 1730869344.94 epoch sec; seqNo:3781 2024-11-06T05:02:24Z- AHRS_M2 msg: 0.01 phi; -0.03 theta; 0.27 psi 2024-11-06T05:02:24Z- handleDepth()- Depth_Keller msg: 1730869344.94 epoch sec; seqNo:4028; depth 47.75 2024-11-06T05:02:24Z- handleDVL() - DVL msg epochMillisec: 1730869344.96 2024-11-06T05:02:24Z- handleDvl() - RDI_Pathfinder msg: 1730869344.96 sec; seqNo:1405; #beams:4 2024-11-06T05:02:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.21 , 43.22 , 42.00 , 43.02 2024-11-06T05:02:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.16 , 0.01 2024-11-06T05:02:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869344.97 epoch sec; seqNo:4028; 4061452.65 north; 594803.00 east; 36.69 lat; -121.94 lon; 47.75 depth; utmZone: 10 2024-11-06T05:02:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869344.15 2024-11-06T05:02:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:25Z- waiting for 1730869348.15, time is 1730869344.97 2024-11-06T05:02:25Z- AHRS_M2 msg: 1730869345.34 epoch sec; seqNo:3782 2024-11-06T05:02:25Z- AHRS_M2 msg: 0.02 phi; -0.03 theta; 0.26 psi 2024-11-06T05:02:25Z- handleDepth()- Depth_Keller msg: 1730869345.35 epoch sec; seqNo:4029; depth 47.73 2024-11-06T05:02:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869345.37 epoch sec; seqNo:4029; 4061453.03 north; 594803.00 east; 36.69 lat; -121.94 lon; 47.73 depth; utmZone: 10 2024-11-06T05:02:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869344.15 2024-11-06T05:02:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:25Z- waiting for 1730869348.15, time is 1730869345.37 2024-11-06T05:02:25Z- AHRS_M2 msg: 1730869345.74 epoch sec; seqNo:3783 2024-11-06T05:02:25Z- AHRS_M2 msg: 0.02 phi; -0.03 theta; 0.25 psi 2024-11-06T05:02:25Z- handleDepth()- Depth_Keller msg: 1730869345.75 epoch sec; seqNo:4030; depth 47.75 2024-11-06T05:02:25Z- handleDVL() - DVL msg epochMillisec: 1730869345.75 2024-11-06T05:02:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:02:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:02:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:02:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:02:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:02:25Z- handleDvl() - RDI_Pathfinder msg: 1730869345.75 sec; seqNo:1406; #beams:0 2024-11-06T05:02:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:02:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.16 , 0.01 2024-11-06T05:02:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869345.77 epoch sec; seqNo:4030; 4061453.41 north; 594803.00 east; 36.69 lat; -121.94 lon; 47.75 depth; utmZone: 10 2024-11-06T05:02:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869344.15 2024-11-06T05:02:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:25Z- waiting for 1730869348.15, time is 1730869345.77 2024-11-06T05:02:26Z- AHRS_M2 msg: 1730869346.15 epoch sec; seqNo:3784 2024-11-06T05:02:26Z- AHRS_M2 msg: 0.02 phi; -0.04 theta; 0.24 psi 2024-11-06T05:02:26Z- handleDepth()- Depth_Keller msg: 1730869346.16 epoch sec; seqNo:4031; depth 47.74 2024-11-06T05:02:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869346.17 epoch sec; seqNo:4031; 4061453.78 north; 594802.99 east; 36.69 lat; -121.94 lon; 47.74 depth; utmZone: 10 2024-11-06T05:02:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869344.15 2024-11-06T05:02:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:26Z- waiting for 1730869348.15, time is 1730869346.17 2024-11-06T05:02:26Z- AHRS_M2 msg: 1730869346.55 epoch sec; seqNo:3785 2024-11-06T05:02:26Z- AHRS_M2 msg: 0.02 phi; -0.04 theta; 0.23 psi 2024-11-06T05:02:26Z- handleDepth()- Depth_Keller msg: 1730869346.56 epoch sec; seqNo:4032; depth 47.75 2024-11-06T05:02:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869346.57 epoch sec; seqNo:4032; 4061454.16 north; 594802.99 east; 36.69 lat; -121.94 lon; 47.75 depth; utmZone: 10 2024-11-06T05:02:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869344.15 2024-11-06T05:02:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:26Z- waiting for 1730869348.15, time is 1730869346.57 2024-11-06T05:02:26Z- AHRS_M2 msg: 1730869346.96 epoch sec; seqNo:3786 2024-11-06T05:02:26Z- AHRS_M2 msg: 0.02 phi; -0.04 theta; 0.22 psi 2024-11-06T05:02:26Z- handleDepth()- Depth_Keller msg: 1730869346.96 epoch sec; seqNo:4033; depth 47.75 2024-11-06T05:02:26Z- handleDVL() - DVL msg epochMillisec: 1730869346.98 2024-11-06T05:02:26Z- handleDvl() - RDI_Pathfinder msg: 1730869346.98 sec; seqNo:1407; #beams:4 2024-11-06T05:02:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.21 , 43.22 , 41.59 , 43.63 2024-11-06T05:02:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.14 , 0.04 2024-11-06T05:02:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869347.00 epoch sec; seqNo:4033; 4061454.54 north; 594802.98 east; 36.69 lat; -121.94 lon; 47.75 depth; utmZone: 10 2024-11-06T05:02:27Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869344.15 2024-11-06T05:02:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:27Z- waiting for 1730869348.15, time is 1730869347.00 2024-11-06T05:02:27Z- AHRS_M2 msg: 1730869347.36 epoch sec; seqNo:3787 2024-11-06T05:02:27Z- AHRS_M2 msg: 0.02 phi; -0.05 theta; 0.21 psi 2024-11-06T05:02:27Z- handleDepth()- Depth_Keller msg: 1730869347.37 epoch sec; seqNo:4034; depth 47.76 2024-11-06T05:02:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869347.39 epoch sec; seqNo:4034; 4061454.92 north; 594802.98 east; 36.69 lat; -121.94 lon; 47.76 depth; utmZone: 10 2024-11-06T05:02:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869344.15 2024-11-06T05:02:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:27Z- waiting for 1730869348.15, time is 1730869347.39 2024-11-06T05:02:27Z- AHRS_M2 msg: 1730869347.76 epoch sec; seqNo:3788 2024-11-06T05:02:27Z- AHRS_M2 msg: 0.02 phi; -0.05 theta; 0.20 psi 2024-11-06T05:02:27Z- handleDepth()- Depth_Keller msg: 1730869347.77 epoch sec; seqNo:4035; depth 47.76 2024-11-06T05:02:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869347.78 epoch sec; seqNo:4035; 4061455.30 north; 594802.98 east; 36.69 lat; -121.94 lon; 47.76 depth; utmZone: 10 2024-11-06T05:02:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869344.15 2024-11-06T05:02:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:27Z- waiting for 1730869348.15, time is 1730869347.78 2024-11-06T05:02:28Z- AHRS_M2 msg: 1730869348.17 epoch sec; seqNo:3789 2024-11-06T05:02:28Z- AHRS_M2 msg: 0.02 phi; -0.05 theta; 0.19 psi 2024-11-06T05:02:28Z- handleDepth()- Depth_Keller msg: 1730869348.18 epoch sec; seqNo:4036; depth 47.77 2024-11-06T05:02:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869348.19 epoch sec; seqNo:4036; 4061455.68 north; 594802.97 east; 36.69 lat; -121.94 lon; 47.77 depth; utmZone: 10 2024-11-06T05:02:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869344.15 2024-11-06T05:02:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:28Z- waiting for 1730869348.15, time is 1730869348.19 2024-11-06T05:02:28Z- LcmTrn::updateTrn() >>>> pose time 1730869348.19, meas time 1730869346.98 <<<< 2024-11-06T05:02:28Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869348.19 2024-11-06T05:02:28Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869346.98 sec, ping # 0; waiting for INS data more recent than 1730869344.15... 2024-11-06T05:02:28Z- TerrainNav::motionUpdate() - currEstimate = (4061456, 594802.97, 47.77) 2024-11-06T05:02:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:02:28Z- TerrainNav:: SubcloudNIS is 0.18, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:02:28Z- TNavPF::Projecting Measurements 2024-11-06T05:02:28Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:02:28Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:02:28Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:02:28Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:02:28Z- TNavFilter::4 good beams. 2024-11-06T05:02:28Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:02:28Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:02:28Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:02:28Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:02:28Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:02:28Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:02:28Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:02:28Z- TerrainMapOctree: (vehN, vehE ) = (4061454.55, 594802.98). 2024-11-06T05:02:28Z- TerrainMapOctree: Min Distance = 211.30. 2024-11-06T05:02:28Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:02:28Z- Forcing Subcloud Comparison 2024-11-06T05:02:28Z- Weighting particles with subcloud comparison 2024-11-06T05:02:28Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:28Z- meanExpectedMeasDiff: -0.586607 delta_rms_squared: 1.360608 alpha: 0.985514 2024-11-06T05:02:28Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:28Z- meanExpectedMeasDiff: 0.256521 delta_rms_squared: 1.299595 alpha: 0.984844 2024-11-06T05:02:28Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:28Z- meanExpectedMeasDiff: -0.427079 delta_rms_squared: 1.266927 alpha: 0.984459 2024-11-06T05:02:28Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:28Z- meanExpectedMeasDiff: 0.242600 delta_rms_squared: 1.398111 alpha: 0.985897 2024-11-06T05:02:28Z- Subcloud NIS: 0.173470 2024-11-06T05:02:28Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:02:28Z- TNavPF:: sumWeights = 0.635618 2024-11-06T05:02:28Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:02:28Z- TNavPF::First Term of Map Covariance Matrix is 0.44 2024-11-06T05:02:28Z- TNavPF::Calculated NIS Value : 0.08 numBeams normalized NIS: 0.02 2024-11-06T05:02:28Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869346.98 sec. 2024-11-06T05:02:28Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:02:28Z- variances([0], [2], and psi_berg) = 0.822028 0.495839 0.000000 2024-11-06T05:02:28Z- publishEstimatesDecoder() - at 1730869348.19 sec 2024-11-06T05:02:28Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:02:28Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061399.77 594808.58 56.12 2024-11-06T05:02:28Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061398.93 594807.44 56.22 lat/lon: 36.69 -121.94 2024-11-06T05:02:28Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.82 0.50 0.74 0.00 2024-11-06T05:02:28Z- AHRS_M2 msg: 1730869348.57 epoch sec; seqNo:3790 2024-11-06T05:02:28Z- AHRS_M2 msg: 0.02 phi; -0.06 theta; 0.18 psi 2024-11-06T05:02:28Z- handleDepth()- Depth_Keller msg: 1730869348.58 epoch sec; seqNo:4037; depth 47.80 2024-11-06T05:02:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869348.59 epoch sec; seqNo:4037; 4061456.06 north; 594802.97 east; 36.69 lat; -121.94 lon; 47.80 depth; utmZone: 10 2024-11-06T05:02:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869348.19 2024-11-06T05:02:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:28Z- waiting for 1730869352.19, time is 1730869348.59 2024-11-06T05:02:28Z- AHRS_M2 msg: 1730869348.97 epoch sec; seqNo:3791 2024-11-06T05:02:28Z- AHRS_M2 msg: 0.02 phi; -0.06 theta; 0.17 psi 2024-11-06T05:02:28Z- handleDepth()- Depth_Keller msg: 1730869348.98 epoch sec; seqNo:4038; depth 47.80 2024-11-06T05:02:29Z- handleDVL() - DVL msg epochMillisec: 1730869349.01 2024-11-06T05:02:29Z- handleDvl() - RDI_Pathfinder msg: 1730869349.01 sec; seqNo:1408; #beams:4 2024-11-06T05:02:29Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.41 , 43.22 , 41.19 , 44.24 2024-11-06T05:02:29Z- handleDvl() - RDI_Pathfinder msg: velocities 0.85 , -0.10 , 0.05 2024-11-06T05:02:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869349.02 epoch sec; seqNo:4038; 4061456.06 north; 594802.97 east; 36.69 lat; -121.94 lon; 47.80 depth; utmZone: 10 2024-11-06T05:02:29Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869348.19 2024-11-06T05:02:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:29Z- waiting for 1730869352.19, time is 1730869349.02 2024-11-06T05:02:29Z- AHRS_M2 msg: 1730869349.38 epoch sec; seqNo:3792 2024-11-06T05:02:29Z- AHRS_M2 msg: 0.03 phi; -0.06 theta; 0.16 psi 2024-11-06T05:02:29Z- handleDepth()- Depth_Keller msg: 1730869349.38 epoch sec; seqNo:4039; depth 47.82 2024-11-06T05:02:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869349.41 epoch sec; seqNo:4039; 4061456.44 north; 594802.96 east; 36.69 lat; -121.94 lon; 47.82 depth; utmZone: 10 2024-11-06T05:02:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869348.19 2024-11-06T05:02:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:29Z- waiting for 1730869352.19, time is 1730869349.41 2024-11-06T05:02:29Z- AHRS_M2 msg: 1730869349.79 epoch sec; seqNo:3793 2024-11-06T05:02:29Z- AHRS_M2 msg: 0.03 phi; -0.07 theta; 0.15 psi 2024-11-06T05:02:29Z- handleDepth()- Depth_Keller msg: 1730869349.79 epoch sec; seqNo:4040; depth 47.82 2024-11-06T05:02:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869349.80 epoch sec; seqNo:4040; 4061456.82 north; 594802.96 east; 36.69 lat; -121.94 lon; 47.82 depth; utmZone: 10 2024-11-06T05:02:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869348.19 2024-11-06T05:02:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:29Z- waiting for 1730869352.19, time is 1730869349.80 2024-11-06T05:02:30Z- AHRS_M2 msg: 1730869350.19 epoch sec; seqNo:3794 2024-11-06T05:02:30Z- AHRS_M2 msg: 0.03 phi; -0.07 theta; 0.14 psi 2024-11-06T05:02:30Z- handleDepth()- Depth_Keller msg: 1730869350.20 epoch sec; seqNo:4041; depth 47.84 2024-11-06T05:02:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869350.21 epoch sec; seqNo:4041; 4061457.19 north; 594802.95 east; 36.69 lat; -121.94 lon; 47.84 depth; utmZone: 10 2024-11-06T05:02:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869348.19 2024-11-06T05:02:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:30Z- waiting for 1730869352.19, time is 1730869350.21 2024-11-06T05:02:30Z- AHRS_M2 msg: 1730869350.59 epoch sec; seqNo:3795 2024-11-06T05:02:30Z- AHRS_M2 msg: 0.03 phi; -0.07 theta; 0.14 psi 2024-11-06T05:02:30Z- handleDepth()- Depth_Keller msg: 1730869350.60 epoch sec; seqNo:4042; depth 47.86 2024-11-06T05:02:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869350.62 epoch sec; seqNo:4042; 4061457.57 north; 594802.95 east; 36.69 lat; -121.94 lon; 47.86 depth; utmZone: 10 2024-11-06T05:02:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869348.19 2024-11-06T05:02:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:30Z- waiting for 1730869352.19, time is 1730869350.62 2024-11-06T05:02:31Z- AHRS_M2 msg: 1730869350.99 epoch sec; seqNo:3796 2024-11-06T05:02:31Z- AHRS_M2 msg: 0.03 phi; -0.07 theta; 0.13 psi 2024-11-06T05:02:31Z- handleDepth()- Depth_Keller msg: 1730869351.00 epoch sec; seqNo:4043; depth 47.86 2024-11-06T05:02:31Z- handleDVL() - DVL msg epochMillisec: 1730869351.02 2024-11-06T05:02:31Z- handleDvl() - RDI_Pathfinder msg: 1730869351.02 sec; seqNo:1409; #beams:4 2024-11-06T05:02:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.61 , 43.02 , 40.78 , 44.65 2024-11-06T05:02:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.05 2024-11-06T05:02:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869351.05 epoch sec; seqNo:4043; 4061457.95 north; 594802.95 east; 36.69 lat; -121.94 lon; 47.86 depth; utmZone: 10 2024-11-06T05:02:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869348.19 2024-11-06T05:02:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:31Z- waiting for 1730869352.19, time is 1730869351.05 2024-11-06T05:02:31Z- AHRS_M2 msg: 1730869351.42 epoch sec; seqNo:3797 2024-11-06T05:02:31Z- AHRS_M2 msg: 0.03 phi; -0.08 theta; 0.12 psi 2024-11-06T05:02:31Z- handleDepth()- Depth_Keller msg: 1730869351.43 epoch sec; seqNo:4044; depth 47.88 2024-11-06T05:02:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869351.44 epoch sec; seqNo:4044; 4061458.33 north; 594802.94 east; 36.69 lat; -121.94 lon; 47.88 depth; utmZone: 10 2024-11-06T05:02:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869348.19 2024-11-06T05:02:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:31Z- waiting for 1730869352.19, time is 1730869351.44 2024-11-06T05:02:31Z- AHRS_M2 msg: 1730869351.82 epoch sec; seqNo:3798 2024-11-06T05:02:31Z- AHRS_M2 msg: 0.03 phi; -0.08 theta; 0.11 psi 2024-11-06T05:02:31Z- handleDepth()- Depth_Keller msg: 1730869351.82 epoch sec; seqNo:4045; depth 47.90 2024-11-06T05:02:31Z- handleDVL() - DVL msg epochMillisec: 1730869351.83 2024-11-06T05:02:31Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:31Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:31Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:31Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:02:31Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:02:31Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:02:31Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:02:31Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:02:31Z- handleDvl() - RDI_Pathfinder msg: 1730869351.83 sec; seqNo:1410; #beams:0 2024-11-06T05:02:31Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:02:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.06 , 0.05 2024-11-06T05:02:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869351.84 epoch sec; seqNo:4045; 4061458.71 north; 594802.94 east; 36.69 lat; -121.94 lon; 47.90 depth; utmZone: 10 2024-11-06T05:02:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869348.19 2024-11-06T05:02:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:31Z- waiting for 1730869352.19, time is 1730869351.84 2024-11-06T05:02:32Z- AHRS_M2 msg: 1730869352.21 epoch sec; seqNo:3799 2024-11-06T05:02:32Z- AHRS_M2 msg: 0.04 phi; -0.08 theta; 0.10 psi 2024-11-06T05:02:32Z- handleDepth()- Depth_Keller msg: 1730869352.22 epoch sec; seqNo:4046; depth 47.92 2024-11-06T05:02:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869352.24 epoch sec; seqNo:4046; 4061459.09 north; 594802.93 east; 36.69 lat; -121.94 lon; 47.92 depth; utmZone: 10 2024-11-06T05:02:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869348.19 2024-11-06T05:02:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:32Z- waiting for 1730869352.19, time is 1730869352.24 2024-11-06T05:02:32Z- LcmTrn::updateTrn() >>>> pose time 1730869352.24, meas time 1730869351.83 <<<< 2024-11-06T05:02:32Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869352.24 2024-11-06T05:02:32Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T05:02:32Z- TerrainNav::motionUpdate() - currEstimate = (4061459, 594802.93, 47.92) 2024-11-06T05:02:32Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:02:32Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:02:32Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T05:02:32Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T05:02:32Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:02:32Z- variances([0], [2], and psi_berg) = 0.823719 0.498270 0.000000 2024-11-06T05:02:32Z- publishEstimatesDecoder() - at 1730869352.24 sec 2024-11-06T05:02:32Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:02:32Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061403.17 594808.55 56.93 2024-11-06T05:02:32Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061402.34 594807.41 56.37 lat/lon: 36.69 -121.94 2024-11-06T05:02:32Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.82 0.50 0.99 0.00 2024-11-06T05:02:32Z- AHRS_M2 msg: 1730869352.61 epoch sec; seqNo:3800 2024-11-06T05:02:32Z- AHRS_M2 msg: 0.04 phi; -0.09 theta; 0.10 psi 2024-11-06T05:02:32Z- handleDepth()- Depth_Keller msg: 1730869352.63 epoch sec; seqNo:4047; depth 47.91 2024-11-06T05:02:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869352.63 epoch sec; seqNo:4047; 4061459.48 north; 594804.15 east; 36.69 lat; -121.94 lon; 47.91 depth; utmZone: 10 2024-11-06T05:02:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869352.24 2024-11-06T05:02:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:32Z- waiting for 1730869356.24, time is 1730869352.63 2024-11-06T05:02:33Z- AHRS_M2 msg: 1730869353.02 epoch sec; seqNo:3801 2024-11-06T05:02:33Z- AHRS_M2 msg: 0.04 phi; -0.09 theta; 0.09 psi 2024-11-06T05:02:33Z- handleDepth()- Depth_Keller msg: 1730869353.02 epoch sec; seqNo:4048; depth 47.93 2024-11-06T05:02:33Z- handleDVL() - DVL msg epochMillisec: 1730869353.05 2024-11-06T05:02:33Z- handleDvl() - RDI_Pathfinder msg: 1730869353.05 sec; seqNo:1411; #beams:4 2024-11-06T05:02:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.61 , 42.82 , 40.57 , 45.06 2024-11-06T05:02:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.02 , 0.03 2024-11-06T05:02:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869353.06 epoch sec; seqNo:4048; 4061459.86 north; 594804.15 east; 36.69 lat; -121.94 lon; 47.93 depth; utmZone: 10 2024-11-06T05:02:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869352.24 2024-11-06T05:02:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:33Z- waiting for 1730869356.24, time is 1730869353.06 2024-11-06T05:02:33Z- AHRS_M2 msg: 1730869353.42 epoch sec; seqNo:3802 2024-11-06T05:02:33Z- AHRS_M2 msg: 0.04 phi; -0.09 theta; 0.08 psi 2024-11-06T05:02:33Z- handleDepth()- Depth_Keller msg: 1730869353.43 epoch sec; seqNo:4049; depth 47.94 2024-11-06T05:02:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869353.45 epoch sec; seqNo:4049; 4061460.24 north; 594804.14 east; 36.69 lat; -121.94 lon; 47.94 depth; utmZone: 10 2024-11-06T05:02:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869352.24 2024-11-06T05:02:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:33Z- waiting for 1730869356.24, time is 1730869353.45 2024-11-06T05:02:33Z- AHRS_M2 msg: 1730869353.83 epoch sec; seqNo:3803 2024-11-06T05:02:33Z- AHRS_M2 msg: 0.04 phi; -0.09 theta; 0.08 psi 2024-11-06T05:02:33Z- handleDepth()- Depth_Keller msg: 1730869353.84 epoch sec; seqNo:4050; depth 47.95 2024-11-06T05:02:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869353.85 epoch sec; seqNo:4050; 4061460.24 north; 594804.14 east; 36.69 lat; -121.94 lon; 47.95 depth; utmZone: 10 2024-11-06T05:02:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869352.24 2024-11-06T05:02:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:33Z- waiting for 1730869356.24, time is 1730869353.85 2024-11-06T05:02:34Z- AHRS_M2 msg: 1730869354.23 epoch sec; seqNo:3804 2024-11-06T05:02:34Z- AHRS_M2 msg: 0.04 phi; -0.10 theta; 0.07 psi 2024-11-06T05:02:34Z- handleDepth()- Depth_Keller msg: 1730869354.23 epoch sec; seqNo:4051; depth 47.97 2024-11-06T05:02:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869354.24 epoch sec; seqNo:4051; 4061460.62 north; 594804.14 east; 36.69 lat; -121.94 lon; 47.97 depth; utmZone: 10 2024-11-06T05:02:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869352.24 2024-11-06T05:02:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:34Z- waiting for 1730869356.24, time is 1730869354.24 2024-11-06T05:02:34Z- AHRS_M2 msg: 1730869354.63 epoch sec; seqNo:3805 2024-11-06T05:02:34Z- AHRS_M2 msg: 0.04 phi; -0.10 theta; 0.06 psi 2024-11-06T05:02:34Z- handleDepth()- Depth_Keller msg: 1730869354.64 epoch sec; seqNo:4052; depth 47.98 2024-11-06T05:02:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869354.66 epoch sec; seqNo:4052; 4061461.00 north; 594804.13 east; 36.69 lat; -121.94 lon; 47.98 depth; utmZone: 10 2024-11-06T05:02:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869352.24 2024-11-06T05:02:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:34Z- waiting for 1730869356.24, time is 1730869354.66 2024-11-06T05:02:35Z- AHRS_M2 msg: 1730869355.03 epoch sec; seqNo:3806 2024-11-06T05:02:35Z- AHRS_M2 msg: 0.04 phi; -0.10 theta; 0.06 psi 2024-11-06T05:02:35Z- handleDepth()- Depth_Keller msg: 1730869355.05 epoch sec; seqNo:4053; depth 48.00 2024-11-06T05:02:35Z- handleDVL() - DVL msg epochMillisec: 1730869355.07 2024-11-06T05:02:35Z- handleDvl() - RDI_Pathfinder msg: 1730869355.07 sec; seqNo:1412; #beams:4 2024-11-06T05:02:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.02 , 42.41 , 39.96 , 45.26 2024-11-06T05:02:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.05 , -0.02 2024-11-06T05:02:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869355.09 epoch sec; seqNo:4053; 4061461.38 north; 594804.13 east; 36.69 lat; -121.94 lon; 48.00 depth; utmZone: 10 2024-11-06T05:02:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869352.24 2024-11-06T05:02:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:35Z- waiting for 1730869356.24, time is 1730869355.09 2024-11-06T05:02:35Z- AHRS_M2 msg: 1730869355.44 epoch sec; seqNo:3807 2024-11-06T05:02:35Z- AHRS_M2 msg: 0.05 phi; -0.10 theta; 0.05 psi 2024-11-06T05:02:35Z- handleDepth()- Depth_Keller msg: 1730869355.45 epoch sec; seqNo:4054; depth 48.02 2024-11-06T05:02:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869355.46 epoch sec; seqNo:4054; 4061461.75 north; 594804.12 east; 36.69 lat; -121.94 lon; 48.02 depth; utmZone: 10 2024-11-06T05:02:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869352.24 2024-11-06T05:02:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:35Z- waiting for 1730869356.24, time is 1730869355.46 2024-11-06T05:02:35Z- AHRS_M2 msg: 1730869355.85 epoch sec; seqNo:3808 2024-11-06T05:02:35Z- AHRS_M2 msg: 0.05 phi; -0.10 theta; 0.04 psi 2024-11-06T05:02:35Z- handleDepth()- Depth_Keller msg: 1730869355.85 epoch sec; seqNo:4055; depth 48.03 2024-11-06T05:02:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869355.86 epoch sec; seqNo:4055; 4061462.13 north; 594804.12 east; 36.69 lat; -121.94 lon; 48.03 depth; utmZone: 10 2024-11-06T05:02:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869352.24 2024-11-06T05:02:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:36Z- waiting for 1730869356.24, time is 1730869355.86 2024-11-06T05:02:36Z- AHRS_M2 msg: 1730869356.25 epoch sec; seqNo:3809 2024-11-06T05:02:36Z- AHRS_M2 msg: 0.05 phi; -0.11 theta; 0.03 psi 2024-11-06T05:02:36Z- handleDepth()- Depth_Keller msg: 1730869356.26 epoch sec; seqNo:4056; depth 48.04 2024-11-06T05:02:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869356.27 epoch sec; seqNo:4056; 4061462.51 north; 594804.12 east; 36.69 lat; -121.94 lon; 48.04 depth; utmZone: 10 2024-11-06T05:02:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869352.24 2024-11-06T05:02:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:36Z- waiting for 1730869356.24, time is 1730869356.27 2024-11-06T05:02:36Z- LcmTrn::updateTrn() >>>> pose time 1730869356.27, meas time 1730869355.07 <<<< 2024-11-06T05:02:36Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869356.27 2024-11-06T05:02:36Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869355.07 sec, ping # 0; waiting for INS data more recent than 1730869352.24... 2024-11-06T05:02:36Z- TerrainNav::motionUpdate() - currEstimate = (4061463, 594804.12, 48.04) 2024-11-06T05:02:36Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:02:36Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:02:36Z- TNavPF::Projecting Measurements 2024-11-06T05:02:36Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:02:36Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:02:36Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:02:36Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:02:36Z- TNavFilter::4 good beams. 2024-11-06T05:02:36Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:02:36Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:02:36Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:02:36Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:02:36Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:02:36Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:02:36Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:02:36Z- TerrainMapOctree: (vehN, vehE ) = (4061461.49, 594803.76). 2024-11-06T05:02:36Z- TerrainMapOctree: Min Distance = 218.22. 2024-11-06T05:02:36Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:02:36Z- Forcing Subcloud Comparison 2024-11-06T05:02:36Z- Weighting particles with subcloud comparison 2024-11-06T05:02:36Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:36Z- meanExpectedMeasDiff: -0.191424 delta_rms_squared: 1.744991 alpha: 0.988668 2024-11-06T05:02:36Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:36Z- meanExpectedMeasDiff: 0.144176 delta_rms_squared: 1.580017 alpha: 0.987500 2024-11-06T05:02:36Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:36Z- meanExpectedMeasDiff: 0.113744 delta_rms_squared: 1.482439 alpha: 0.986688 2024-11-06T05:02:36Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:36Z- meanExpectedMeasDiff: -0.634078 delta_rms_squared: 1.774898 alpha: 0.988857 2024-11-06T05:02:36Z- Subcloud NIS: 0.170596 2024-11-06T05:02:36Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:02:36Z- TNavPF:: sumWeights = 0.612651 2024-11-06T05:02:36Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:02:36Z- TNavPF::First Term of Map Covariance Matrix is 0.46 2024-11-06T05:02:36Z- TNavPF::Calculated NIS Value : 0.05 numBeams normalized NIS: 0.01 2024-11-06T05:02:36Z- TerrainNav::Resampling particle filter... 2024-11-06T05:02:36Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869355.07 sec. 2024-11-06T05:02:36Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:02:36Z- variances([0], [2], and psi_berg) = 0.815312 0.498403 0.000000 2024-11-06T05:02:36Z- publishEstimatesDecoder() - at 1730869356.27 sec 2024-11-06T05:02:36Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:02:36Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061408.42 594807.49 55.61 2024-11-06T05:02:36Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061405.74 594808.59 56.57 lat/lon: 36.69 -121.94 2024-11-06T05:02:36Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.82 0.50 0.79 0.00 2024-11-06T05:02:36Z- AHRS_M2 msg: 1730869356.65 epoch sec; seqNo:3810 2024-11-06T05:02:36Z- AHRS_M2 msg: 0.04 phi; -0.11 theta; 0.03 psi 2024-11-06T05:02:36Z- handleDepth()- Depth_Keller msg: 1730869356.66 epoch sec; seqNo:4057; depth 48.06 2024-11-06T05:02:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869356.67 epoch sec; seqNo:4057; 4061462.89 north; 594804.11 east; 36.69 lat; -121.94 lon; 48.06 depth; utmZone: 10 2024-11-06T05:02:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869356.27 2024-11-06T05:02:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:36Z- waiting for 1730869360.27, time is 1730869356.67 2024-11-06T05:02:37Z- handleDepth()- Depth_Keller msg: 1730869357.08 epoch sec; seqNo:4058; depth 48.08 2024-11-06T05:02:37Z- handleDVL() - DVL msg epochMillisec: 1730869357.10 2024-11-06T05:02:37Z- handleDvl() - RDI_Pathfinder msg: 1730869357.10 sec; seqNo:1413; #beams:4 2024-11-06T05:02:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.61 , 42.21 , 39.55 , 45.47 2024-11-06T05:02:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.13 , -0.06 2024-11-06T05:02:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869357.11 epoch sec; seqNo:4058; 4061463.27 north; 594804.11 east; 36.69 lat; -121.94 lon; 48.08 depth; utmZone: 10 2024-11-06T05:02:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869356.27 2024-11-06T05:02:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:37Z- waiting for 1730869360.27, time is 1730869357.11 2024-11-06T05:02:37Z- AHRS_M2 msg: 1730869357.46 epoch sec; seqNo:3811 2024-11-06T05:02:37Z- AHRS_M2 msg: 0.04 phi; -0.11 theta; 0.01 psi 2024-11-06T05:02:37Z- handleDepth()- Depth_Keller msg: 1730869357.46 epoch sec; seqNo:4059; depth 48.10 2024-11-06T05:02:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869357.48 epoch sec; seqNo:4059; 4061463.63 north; 594802.88 east; 36.69 lat; -121.94 lon; 48.10 depth; utmZone: 10 2024-11-06T05:02:37Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869356.27 2024-11-06T05:02:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:37Z- waiting for 1730869360.27, time is 1730869357.48 2024-11-06T05:02:37Z- AHRS_M2 msg: 1730869357.88 epoch sec; seqNo:3812 2024-11-06T05:02:37Z- AHRS_M2 msg: 0.04 phi; -0.11 theta; 6.28 psi 2024-11-06T05:02:37Z- handleDepth()- Depth_Keller msg: 1730869357.88 epoch sec; seqNo:4060; depth 48.13 2024-11-06T05:02:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869357.89 epoch sec; seqNo:4060; 4061464.01 north; 594802.88 east; 36.69 lat; -121.94 lon; 48.13 depth; utmZone: 10 2024-11-06T05:02:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869356.27 2024-11-06T05:02:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:38Z- waiting for 1730869360.27, time is 1730869357.89 2024-11-06T05:02:38Z- AHRS_M2 msg: 1730869358.27 epoch sec; seqNo:3813 2024-11-06T05:02:38Z- AHRS_M2 msg: 0.04 phi; -0.12 theta; 6.27 psi 2024-11-06T05:02:38Z- handleDepth()- Depth_Keller msg: 1730869358.28 epoch sec; seqNo:4061; depth 48.16 2024-11-06T05:02:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869358.29 epoch sec; seqNo:4061; 4061464.39 north; 594802.87 east; 36.69 lat; -121.94 lon; 48.16 depth; utmZone: 10 2024-11-06T05:02:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869356.27 2024-11-06T05:02:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:38Z- waiting for 1730869360.27, time is 1730869358.29 2024-11-06T05:02:38Z- AHRS_M2 msg: 1730869358.70 epoch sec; seqNo:3814 2024-11-06T05:02:38Z- AHRS_M2 msg: 0.04 phi; -0.12 theta; 6.26 psi 2024-11-06T05:02:38Z- handleDepth()- Depth_Keller msg: 1730869358.72 epoch sec; seqNo:4062; depth 48.17 2024-11-06T05:02:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869358.77 epoch sec; seqNo:4062; 4061464.77 north; 594802.87 east; 36.69 lat; -121.94 lon; 48.17 depth; utmZone: 10 2024-11-06T05:02:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869356.27 2024-11-06T05:02:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:38Z- waiting for 1730869360.27, time is 1730869358.77 2024-11-06T05:02:39Z- handleDepth()- Depth_Keller msg: 1730869359.09 epoch sec; seqNo:4063; depth 48.21 2024-11-06T05:02:39Z- handleDVL() - DVL msg epochMillisec: 1730869359.12 2024-11-06T05:02:39Z- handleDvl() - RDI_Pathfinder msg: 1730869359.12 sec; seqNo:1414; #beams:4 2024-11-06T05:02:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.21 , 42.00 , 38.94 , 45.47 2024-11-06T05:02:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.19 , -0.02 2024-11-06T05:02:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869359.14 epoch sec; seqNo:4063; 4061465.15 north; 594802.87 east; 36.69 lat; -121.94 lon; 48.21 depth; utmZone: 10 2024-11-06T05:02:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869356.27 2024-11-06T05:02:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:39Z- waiting for 1730869360.27, time is 1730869359.14 2024-11-06T05:02:39Z- AHRS_M2 msg: 1730869359.48 epoch sec; seqNo:3815 2024-11-06T05:02:39Z- AHRS_M2 msg: 0.04 phi; -0.12 theta; 6.24 psi 2024-11-06T05:02:39Z- handleDepth()- Depth_Keller msg: 1730869359.48 epoch sec; seqNo:4064; depth 48.26 2024-11-06T05:02:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869359.50 epoch sec; seqNo:4064; 4061465.15 north; 594802.87 east; 36.69 lat; -121.94 lon; 48.26 depth; utmZone: 10 2024-11-06T05:02:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869356.27 2024-11-06T05:02:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:39Z- waiting for 1730869360.27, time is 1730869359.50 2024-11-06T05:02:39Z- handleDepth()- Depth_Keller msg: 1730869359.91 epoch sec; seqNo:4065; depth 48.29 2024-11-06T05:02:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869359.92 epoch sec; seqNo:4065; 4061465.53 north; 594802.86 east; 36.69 lat; -121.94 lon; 48.29 depth; utmZone: 10 2024-11-06T05:02:40Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:02:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869356.27 2024-11-06T05:02:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:40Z- waiting for 1730869360.27, time is 1730869359.92 2024-11-06T05:02:40Z- AHRS_M2 msg: 1730869360.30 epoch sec; seqNo:3816 2024-11-06T05:02:40Z- AHRS_M2 msg: 0.05 phi; -0.13 theta; 6.22 psi 2024-11-06T05:02:40Z- handleDepth()- Depth_Keller msg: 1730869360.31 epoch sec; seqNo:4066; depth 48.33 2024-11-06T05:02:40Z- handleDVL() - DVL msg epochMillisec: 1730869360.31 2024-11-06T05:02:40Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:40Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:40Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:40Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:02:40Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:02:40Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:02:40Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:02:40Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:02:40Z- handleDvl() - RDI_Pathfinder msg: 1730869360.31 sec; seqNo:1415; #beams:0 2024-11-06T05:02:40Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:02:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.19 , -0.02 2024-11-06T05:02:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869360.32 epoch sec; seqNo:4066; 4061465.91 north; 594802.86 east; 36.69 lat; -121.94 lon; 48.33 depth; utmZone: 10 2024-11-06T05:02:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869356.27 2024-11-06T05:02:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:40Z- waiting for 1730869360.27, time is 1730869360.32 2024-11-06T05:02:40Z- LcmTrn::updateTrn() >>>> pose time 1730869360.32, meas time 1730869360.31 <<<< 2024-11-06T05:02:40Z- LcmTrn::updateTrn() equating pose 1730869360.31 and meas times 1730869360.31 2024-11-06T05:02:40Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869360.31 2024-11-06T05:02:40Z- TerrainNav::motionUpdate() - currEstimate = (4061466, 594802.86, 48.33) 2024-11-06T05:02:40Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:02:40Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:02:40Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T05:02:40Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T05:02:40Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T05:02:40Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:02:40Z- variances([0], [2], and psi_berg) = 0.816905 0.500875 0.000000 2024-11-06T05:02:40Z- publishEstimatesDecoder() - at 1730869360.31 sec 2024-11-06T05:02:40Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:02:40Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061411.76 594806.20 55.22 2024-11-06T05:02:40Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061409.14 594807.33 56.86 lat/lon: 36.69 -121.94 2024-11-06T05:02:40Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.82 0.50 1.04 0.00 2024-11-06T05:02:40Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T05:02:40Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T05:02:40Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T05:02:40Z- AHRS_M2 msg: 1730869360.70 epoch sec; seqNo:3817 2024-11-06T05:02:40Z- AHRS_M2 msg: 0.05 phi; -0.13 theta; 6.22 psi 2024-11-06T05:02:40Z- handleDepth()- Depth_Keller msg: 1730869360.76 epoch sec; seqNo:4067; depth 48.35 2024-11-06T05:02:40Z- handleDVL() - DVL msg epochMillisec: 1730869360.78 2024-11-06T05:02:40Z- handleDvl() - RDI_Pathfinder msg: 1730869360.78 sec; seqNo:1416; #beams:4 2024-11-06T05:02:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.41 , 41.59 , 38.74 , 45.67 2024-11-06T05:02:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.19 , 0.07 2024-11-06T05:02:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869360.80 epoch sec; seqNo:4067; 4061466.67 north; 594802.85 east; 36.69 lat; -121.94 lon; 48.35 depth; utmZone: 10 2024-11-06T05:02:40Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869360.31 2024-11-06T05:02:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:40Z- waiting for 1730869364.31, time is 1730869360.80 2024-11-06T05:02:41Z- AHRS_M2 msg: 1730869361.10 epoch sec; seqNo:3818 2024-11-06T05:02:41Z- AHRS_M2 msg: 0.06 phi; -0.13 theta; 6.21 psi 2024-11-06T05:02:41Z- handleDepth()- Depth_Keller msg: 1730869361.10 epoch sec; seqNo:4068; depth 48.40 2024-11-06T05:02:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869361.12 epoch sec; seqNo:4068; 4061466.67 north; 594802.85 east; 36.69 lat; -121.94 lon; 48.40 depth; utmZone: 10 2024-11-06T05:02:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869360.31 2024-11-06T05:02:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:41Z- waiting for 1730869364.31, time is 1730869361.12 2024-11-06T05:02:41Z- AHRS_M2 msg: 1730869361.50 epoch sec; seqNo:3819 2024-11-06T05:02:41Z- AHRS_M2 msg: 0.07 phi; -0.13 theta; 6.20 psi 2024-11-06T05:02:41Z- handleDepth()- Depth_Keller msg: 1730869361.51 epoch sec; seqNo:4069; depth 48.44 2024-11-06T05:02:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869361.52 epoch sec; seqNo:4069; 4061467.04 north; 594802.85 east; 36.69 lat; -121.94 lon; 48.44 depth; utmZone: 10 2024-11-06T05:02:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869360.31 2024-11-06T05:02:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:41Z- waiting for 1730869364.31, time is 1730869361.52 2024-11-06T05:02:41Z- AHRS_M2 msg: 1730869361.91 epoch sec; seqNo:3820 2024-11-06T05:02:41Z- AHRS_M2 msg: 0.08 phi; -0.13 theta; 6.20 psi 2024-11-06T05:02:41Z- handleDepth()- Depth_Keller msg: 1730869361.91 epoch sec; seqNo:4070; depth 48.49 2024-11-06T05:02:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869361.93 epoch sec; seqNo:4070; 4061467.42 north; 594802.84 east; 36.69 lat; -121.94 lon; 48.49 depth; utmZone: 10 2024-11-06T05:02:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869360.31 2024-11-06T05:02:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:42Z- waiting for 1730869364.31, time is 1730869361.93 2024-11-06T05:02:42Z- AHRS_M2 msg: 1730869362.31 epoch sec; seqNo:3821 2024-11-06T05:02:42Z- AHRS_M2 msg: 0.09 phi; -0.14 theta; 6.20 psi 2024-11-06T05:02:42Z- handleDepth()- Depth_Keller msg: 1730869362.32 epoch sec; seqNo:4071; depth 48.53 2024-11-06T05:02:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869362.32 epoch sec; seqNo:4071; 4061467.80 north; 594802.84 east; 36.69 lat; -121.94 lon; 48.53 depth; utmZone: 10 2024-11-06T05:02:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869360.31 2024-11-06T05:02:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:42Z- waiting for 1730869364.31, time is 1730869362.32 2024-11-06T05:02:42Z- AHRS_M2 msg: 1730869362.75 epoch sec; seqNo:3822 2024-11-06T05:02:42Z- AHRS_M2 msg: 0.09 phi; -0.14 theta; 6.20 psi 2024-11-06T05:02:42Z- handleDepth()- Depth_Keller msg: 1730869362.76 epoch sec; seqNo:4072; depth 48.59 2024-11-06T05:02:42Z- handleDVL() - DVL msg epochMillisec: 1730869362.78 2024-11-06T05:02:42Z- handleDvl() - RDI_Pathfinder msg: 1730869362.78 sec; seqNo:1417; #beams:4 2024-11-06T05:02:42Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.43 , 40.78 , 38.53 , 46.08 2024-11-06T05:02:42Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.10 , 0.10 2024-11-06T05:02:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869362.80 epoch sec; seqNo:4072; 4061468.18 north; 594802.83 east; 36.69 lat; -121.94 lon; 48.59 depth; utmZone: 10 2024-11-06T05:02:42Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869360.31 2024-11-06T05:02:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:42Z- waiting for 1730869364.31, time is 1730869362.80 2024-11-06T05:02:43Z- AHRS_M2 msg: 1730869363.13 epoch sec; seqNo:3823 2024-11-06T05:02:43Z- AHRS_M2 msg: 0.10 phi; -0.14 theta; 6.20 psi 2024-11-06T05:02:43Z- handleDepth()- Depth_Keller msg: 1730869363.14 epoch sec; seqNo:4073; depth 48.64 2024-11-06T05:02:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869363.16 epoch sec; seqNo:4073; 4061468.55 north; 594801.61 east; 36.69 lat; -121.94 lon; 48.64 depth; utmZone: 10 2024-11-06T05:02:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869360.31 2024-11-06T05:02:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:43Z- waiting for 1730869364.31, time is 1730869363.16 2024-11-06T05:02:43Z- AHRS_M2 msg: 1730869363.53 epoch sec; seqNo:3824 2024-11-06T05:02:43Z- AHRS_M2 msg: 0.10 phi; -0.14 theta; 6.20 psi 2024-11-06T05:02:43Z- handleDepth()- Depth_Keller msg: 1730869363.54 epoch sec; seqNo:4074; depth 48.68 2024-11-06T05:02:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869363.55 epoch sec; seqNo:4074; 4061468.93 north; 594801.60 east; 36.69 lat; -121.94 lon; 48.68 depth; utmZone: 10 2024-11-06T05:02:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869360.31 2024-11-06T05:02:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:43Z- waiting for 1730869364.31, time is 1730869363.55 2024-11-06T05:02:43Z- AHRS_M2 msg: 1730869363.94 epoch sec; seqNo:3825 2024-11-06T05:02:43Z- AHRS_M2 msg: 0.11 phi; -0.14 theta; 6.20 psi 2024-11-06T05:02:43Z- handleDepth()- Depth_Keller msg: 1730869363.94 epoch sec; seqNo:4075; depth 48.71 2024-11-06T05:02:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869363.95 epoch sec; seqNo:4075; 4061469.30 north; 594801.60 east; 36.69 lat; -121.94 lon; 48.71 depth; utmZone: 10 2024-11-06T05:02:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869360.31 2024-11-06T05:02:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:44Z- waiting for 1730869364.31, time is 1730869363.95 2024-11-06T05:02:44Z- AHRS_M2 msg: 1730869364.35 epoch sec; seqNo:3826 2024-11-06T05:02:44Z- AHRS_M2 msg: 0.11 phi; -0.14 theta; 6.20 psi 2024-11-06T05:02:44Z- handleDepth()- Depth_Keller msg: 1730869364.35 epoch sec; seqNo:4076; depth 48.76 2024-11-06T05:02:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869364.36 epoch sec; seqNo:4076; 4061469.68 north; 594801.60 east; 36.69 lat; -121.94 lon; 48.76 depth; utmZone: 10 2024-11-06T05:02:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869360.31 2024-11-06T05:02:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:44Z- waiting for 1730869364.31, time is 1730869364.36 2024-11-06T05:02:44Z- LcmTrn::updateTrn() >>>> pose time 1730869364.36, meas time 1730869362.78 <<<< 2024-11-06T05:02:44Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869364.36 2024-11-06T05:02:44Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869362.78 sec, ping # 0; waiting for INS data more recent than 1730869360.31... 2024-11-06T05:02:44Z- TerrainNav::motionUpdate() - currEstimate = (4061470, 594801.60, 48.76) 2024-11-06T05:02:44Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:02:44Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:02:44Z- TNavPF::Projecting Measurements 2024-11-06T05:02:44Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:02:44Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:02:44Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:02:44Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:02:44Z- TNavFilter::4 good beams. 2024-11-06T05:02:44Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:02:44Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:02:44Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:02:44Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:02:44Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:02:44Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:02:44Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:02:44Z- TerrainMapOctree: (vehN, vehE ) = (4061468.21, 594802.09). 2024-11-06T05:02:44Z- TerrainMapOctree: Min Distance = 224.34. 2024-11-06T05:02:44Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:02:44Z- Forcing Subcloud Comparison 2024-11-06T05:02:44Z- Weighting particles with subcloud comparison 2024-11-06T05:02:44Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:44Z- meanExpectedMeasDiff: -0.342167 delta_rms_squared: 1.937896 alpha: 0.989785 2024-11-06T05:02:44Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:44Z- meanExpectedMeasDiff: -0.075375 delta_rms_squared: 1.601411 alpha: 0.987665 2024-11-06T05:02:44Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:44Z- meanExpectedMeasDiff: -0.032865 delta_rms_squared: 1.508019 alpha: 0.986911 2024-11-06T05:02:44Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:44Z- meanExpectedMeasDiff: -0.173082 delta_rms_squared: 2.210396 alpha: 0.991033 2024-11-06T05:02:44Z- Subcloud NIS: 0.166636 2024-11-06T05:02:44Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:02:44Z- TNavPF:: sumWeights = 0.614826 2024-11-06T05:02:44Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:02:44Z- TNavPF::First Term of Map Covariance Matrix is 0.48 2024-11-06T05:02:44Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T05:02:44Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869362.78 sec. 2024-11-06T05:02:44Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:02:44Z- variances([0], [2], and psi_berg) = 0.814920 0.510487 0.000000 2024-11-06T05:02:44Z- publishEstimatesDecoder() - at 1730869364.36 sec 2024-11-06T05:02:44Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:02:44Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061413.41 594805.22 56.18 2024-11-06T05:02:44Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061412.94 594806.07 57.41 lat/lon: 36.69 -121.94 2024-11-06T05:02:44Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.81 0.51 0.75 0.00 2024-11-06T05:02:44Z- AHRS_M2 msg: 1730869364.77 epoch sec; seqNo:3827 2024-11-06T05:02:44Z- AHRS_M2 msg: 0.11 phi; -0.15 theta; 6.20 psi 2024-11-06T05:02:44Z- handleDepth()- Depth_Keller msg: 1730869364.78 epoch sec; seqNo:4077; depth 48.82 2024-11-06T05:02:44Z- handleDVL() - DVL msg epochMillisec: 1730869364.84 2024-11-06T05:02:44Z- handleDvl() - RDI_Pathfinder msg: 1730869364.84 sec; seqNo:1418; #beams:4 2024-11-06T05:02:44Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 44.24 , 40.17 , 38.33 , 46.28 2024-11-06T05:02:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , 0.05 2024-11-06T05:02:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869364.86 epoch sec; seqNo:4077; 4061470.06 north; 594801.59 east; 36.69 lat; -121.94 lon; 48.82 depth; utmZone: 10 2024-11-06T05:02:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869364.36 2024-11-06T05:02:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:44Z- waiting for 1730869368.36, time is 1730869364.86 2024-11-06T05:02:45Z- AHRS_M2 msg: 1730869365.15 epoch sec; seqNo:3828 2024-11-06T05:02:45Z- AHRS_M2 msg: 0.11 phi; -0.15 theta; 6.19 psi 2024-11-06T05:02:45Z- handleDepth()- Depth_Keller msg: 1730869365.16 epoch sec; seqNo:4078; depth 48.86 2024-11-06T05:02:45Z- handleDVL() - DVL msg epochMillisec: 1730869365.17 2024-11-06T05:02:45Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:45Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:45Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:45Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:02:45Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:02:45Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:02:45Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:02:45Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:02:45Z- handleDvl() - RDI_Pathfinder msg: 1730869365.17 sec; seqNo:1419; #beams:0 2024-11-06T05:02:45Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:02:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , 0.05 2024-11-06T05:02:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869365.19 epoch sec; seqNo:4078; 4061470.44 north; 594801.59 east; 36.69 lat; -121.94 lon; 48.86 depth; utmZone: 10 2024-11-06T05:02:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869364.36 2024-11-06T05:02:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:45Z- waiting for 1730869368.36, time is 1730869365.19 2024-11-06T05:02:45Z- AHRS_M2 msg: 1730869365.55 epoch sec; seqNo:3829 2024-11-06T05:02:45Z- AHRS_M2 msg: 0.10 phi; -0.15 theta; 6.19 psi 2024-11-06T05:02:45Z- handleDepth()- Depth_Keller msg: 1730869365.56 epoch sec; seqNo:4079; depth 48.89 2024-11-06T05:02:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869365.57 epoch sec; seqNo:4079; 4061470.82 north; 594801.58 east; 36.69 lat; -121.94 lon; 48.89 depth; utmZone: 10 2024-11-06T05:02:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869364.36 2024-11-06T05:02:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:45Z- waiting for 1730869368.36, time is 1730869365.57 2024-11-06T05:02:45Z- AHRS_M2 msg: 1730869365.95 epoch sec; seqNo:3830 2024-11-06T05:02:45Z- AHRS_M2 msg: 0.10 phi; -0.15 theta; 6.19 psi 2024-11-06T05:02:45Z- handleDepth()- Depth_Keller msg: 1730869365.97 epoch sec; seqNo:4080; depth 48.93 2024-11-06T05:02:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869365.98 epoch sec; seqNo:4080; 4061471.20 north; 594801.58 east; 36.69 lat; -121.94 lon; 48.93 depth; utmZone: 10 2024-11-06T05:02:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869364.36 2024-11-06T05:02:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:46Z- waiting for 1730869368.36, time is 1730869365.98 2024-11-06T05:02:46Z- AHRS_M2 msg: 1730869366.36 epoch sec; seqNo:3831 2024-11-06T05:02:46Z- AHRS_M2 msg: 0.10 phi; -0.15 theta; 6.19 psi 2024-11-06T05:02:46Z- handleDepth()- Depth_Keller msg: 1730869366.36 epoch sec; seqNo:4081; depth 48.97 2024-11-06T05:02:46Z- handleDVL() - DVL msg epochMillisec: 1730869366.37 2024-11-06T05:02:46Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:46Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:46Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:46Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:02:46Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:02:46Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:02:46Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:02:46Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:02:46Z- handleDvl() - RDI_Pathfinder msg: 1730869366.37 sec; seqNo:1420; #beams:0 2024-11-06T05:02:46Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:02:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , 0.05 2024-11-06T05:02:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869366.37 epoch sec; seqNo:4081; 4061471.20 north; 594801.58 east; 36.69 lat; -121.94 lon; 48.97 depth; utmZone: 10 2024-11-06T05:02:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869364.36 2024-11-06T05:02:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:46Z- waiting for 1730869368.36, time is 1730869366.37 2024-11-06T05:02:46Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T05:02:46Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T05:02:46Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T05:02:46Z- AHRS_M2 msg: 1730869366.76 epoch sec; seqNo:3832 2024-11-06T05:02:46Z- AHRS_M2 msg: 0.10 phi; -0.15 theta; 6.19 psi 2024-11-06T05:02:46Z- handleDepth()- Depth_Keller msg: 1730869366.79 epoch sec; seqNo:4082; depth 49.01 2024-11-06T05:02:46Z- handleDVL() - DVL msg epochMillisec: 1730869366.82 2024-11-06T05:02:46Z- handleDvl() - RDI_Pathfinder msg: 1730869366.82 sec; seqNo:1421; #beams:4 2024-11-06T05:02:46Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.84 , 39.96 , 37.72 , 46.08 2024-11-06T05:02:46Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , -0.04 2024-11-06T05:02:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869366.85 epoch sec; seqNo:4082; 4061471.58 north; 594801.57 east; 36.69 lat; -121.94 lon; 49.01 depth; utmZone: 10 2024-11-06T05:02:46Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869364.36 2024-11-06T05:02:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:46Z- waiting for 1730869368.36, time is 1730869366.85 2024-11-06T05:02:47Z- AHRS_M2 msg: 1730869367.21 epoch sec; seqNo:3833 2024-11-06T05:02:47Z- AHRS_M2 msg: 0.10 phi; -0.15 theta; 6.19 psi 2024-11-06T05:02:47Z- handleDepth()- Depth_Keller msg: 1730869367.21 epoch sec; seqNo:4083; depth 49.06 2024-11-06T05:02:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869367.24 epoch sec; seqNo:4083; 4061471.96 north; 594801.57 east; 36.69 lat; -121.94 lon; 49.06 depth; utmZone: 10 2024-11-06T05:02:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869364.36 2024-11-06T05:02:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:47Z- waiting for 1730869368.36, time is 1730869367.24 2024-11-06T05:02:47Z- AHRS_M2 msg: 1730869367.61 epoch sec; seqNo:3834 2024-11-06T05:02:47Z- AHRS_M2 msg: 0.10 phi; -0.15 theta; 6.18 psi 2024-11-06T05:02:47Z- handleDepth()- Depth_Keller msg: 1730869367.61 epoch sec; seqNo:4084; depth 49.09 2024-11-06T05:02:47Z- handleDVL() - DVL msg epochMillisec: 1730869367.62 2024-11-06T05:02:47Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:47Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:47Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:47Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:02:47Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:02:47Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:02:47Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:02:47Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:02:47Z- handleDvl() - RDI_Pathfinder msg: 1730869367.62 sec; seqNo:1422; #beams:0 2024-11-06T05:02:47Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:02:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , -0.01 , -0.04 2024-11-06T05:02:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869367.62 epoch sec; seqNo:4084; 4061472.33 north; 594801.57 east; 36.69 lat; -121.94 lon; 49.09 depth; utmZone: 10 2024-11-06T05:02:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869364.36 2024-11-06T05:02:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:47Z- waiting for 1730869368.36, time is 1730869367.62 2024-11-06T05:02:48Z- AHRS_M2 msg: 1730869368.02 epoch sec; seqNo:3835 2024-11-06T05:02:48Z- AHRS_M2 msg: 0.10 phi; -0.15 theta; 6.18 psi 2024-11-06T05:02:48Z- handleDepth()- Depth_Keller msg: 1730869368.03 epoch sec; seqNo:4085; depth 49.13 2024-11-06T05:02:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869368.04 epoch sec; seqNo:4085; 4061472.71 north; 594801.56 east; 36.69 lat; -121.94 lon; 49.13 depth; utmZone: 10 2024-11-06T05:02:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869364.36 2024-11-06T05:02:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:48Z- waiting for 1730869368.36, time is 1730869368.04 2024-11-06T05:02:48Z- AHRS_M2 msg: 1730869368.42 epoch sec; seqNo:3836 2024-11-06T05:02:48Z- AHRS_M2 msg: 0.09 phi; -0.15 theta; 6.18 psi 2024-11-06T05:02:48Z- handleDepth()- Depth_Keller msg: 1730869368.43 epoch sec; seqNo:4086; depth 49.17 2024-11-06T05:02:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869368.44 epoch sec; seqNo:4086; 4061473.09 north; 594801.56 east; 36.69 lat; -121.94 lon; 49.17 depth; utmZone: 10 2024-11-06T05:02:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869364.36 2024-11-06T05:02:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:48Z- waiting for 1730869368.36, time is 1730869368.44 2024-11-06T05:02:48Z- LcmTrn::updateTrn() >>>> pose time 1730869368.44, meas time 1730869367.62 <<<< 2024-11-06T05:02:48Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869368.44 2024-11-06T05:02:48Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T05:02:48Z- TerrainNav::motionUpdate() - currEstimate = (4061473, 594801.56, 49.17) 2024-11-06T05:02:48Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:02:48Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:02:48Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T05:02:48Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T05:02:48Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:02:48Z- variances([0], [2], and psi_berg) = 0.817639 0.513188 0.000000 2024-11-06T05:02:48Z- publishEstimatesDecoder() - at 1730869368.44 sec 2024-11-06T05:02:48Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:02:48Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061416.85 594805.23 56.10 2024-11-06T05:02:48Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061416.35 594806.03 57.81 lat/lon: 36.69 -121.94 2024-11-06T05:02:48Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.82 0.51 0.98 0.00 2024-11-06T05:02:48Z- AHRS_M2 msg: 1730869368.82 epoch sec; seqNo:3837 2024-11-06T05:02:48Z- AHRS_M2 msg: 0.09 phi; -0.14 theta; 6.18 psi 2024-11-06T05:02:48Z- handleDepth()- Depth_Keller msg: 1730869368.83 epoch sec; seqNo:4087; depth 49.21 2024-11-06T05:02:48Z- handleDVL() - DVL msg epochMillisec: 1730869368.87 2024-11-06T05:02:48Z- handleDvl() - RDI_Pathfinder msg: 1730869368.87 sec; seqNo:1423; #beams:4 2024-11-06T05:02:48Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 43.22 , 39.76 , 37.51 , 46.08 2024-11-06T05:02:48Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.08 , -0.05 2024-11-06T05:02:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869368.88 epoch sec; seqNo:4087; 4061473.47 north; 594801.55 east; 36.69 lat; -121.94 lon; 49.21 depth; utmZone: 10 2024-11-06T05:02:48Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869368.44 2024-11-06T05:02:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:48Z- waiting for 1730869372.44, time is 1730869368.88 2024-11-06T05:02:49Z- AHRS_M2 msg: 1730869369.23 epoch sec; seqNo:3838 2024-11-06T05:02:49Z- AHRS_M2 msg: 0.09 phi; -0.14 theta; 6.17 psi 2024-11-06T05:02:49Z- handleDepth()- Depth_Keller msg: 1730869369.23 epoch sec; seqNo:4088; depth 49.24 2024-11-06T05:02:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869369.26 epoch sec; seqNo:4088; 4061473.85 north; 594801.55 east; 36.69 lat; -121.94 lon; 49.24 depth; utmZone: 10 2024-11-06T05:02:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869368.44 2024-11-06T05:02:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:49Z- waiting for 1730869372.44, time is 1730869369.26 2024-11-06T05:02:49Z- AHRS_M2 msg: 1730869369.63 epoch sec; seqNo:3839 2024-11-06T05:02:49Z- AHRS_M2 msg: 0.10 phi; -0.14 theta; 6.17 psi 2024-11-06T05:02:49Z- handleDepth()- Depth_Keller msg: 1730869369.64 epoch sec; seqNo:4089; depth 49.28 2024-11-06T05:02:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869369.65 epoch sec; seqNo:4089; 4061474.23 north; 594801.55 east; 36.69 lat; -121.94 lon; 49.28 depth; utmZone: 10 2024-11-06T05:02:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869368.44 2024-11-06T05:02:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:49Z- waiting for 1730869372.44, time is 1730869369.65 2024-11-06T05:02:50Z- AHRS_M2 msg: 1730869370.03 epoch sec; seqNo:3840 2024-11-06T05:02:50Z- AHRS_M2 msg: 0.10 phi; -0.14 theta; 6.17 psi 2024-11-06T05:02:50Z- handleDepth()- Depth_Keller msg: 1730869370.04 epoch sec; seqNo:4090; depth 49.31 2024-11-06T05:02:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869370.06 epoch sec; seqNo:4090; 4061474.61 north; 594801.54 east; 36.69 lat; -121.94 lon; 49.31 depth; utmZone: 10 2024-11-06T05:02:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869368.44 2024-11-06T05:02:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:50Z- waiting for 1730869372.44, time is 1730869370.06 2024-11-06T05:02:50Z- AHRS_M2 msg: 1730869370.44 epoch sec; seqNo:3841 2024-11-06T05:02:50Z- AHRS_M2 msg: 0.09 phi; -0.14 theta; 6.16 psi 2024-11-06T05:02:50Z- handleDepth()- Depth_Keller msg: 1730869370.44 epoch sec; seqNo:4091; depth 49.36 2024-11-06T05:02:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869370.45 epoch sec; seqNo:4091; 4061474.99 north; 594801.54 east; 36.69 lat; -121.94 lon; 49.36 depth; utmZone: 10 2024-11-06T05:02:50Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869368.44 2024-11-06T05:02:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:50Z- waiting for 1730869372.44, time is 1730869370.45 2024-11-06T05:02:50Z- AHRS_M2 msg: 1730869370.84 epoch sec; seqNo:3842 2024-11-06T05:02:50Z- AHRS_M2 msg: 0.09 phi; -0.14 theta; 6.16 psi 2024-11-06T05:02:50Z- handleDepth()- Depth_Keller msg: 1730869370.85 epoch sec; seqNo:4092; depth 49.40 2024-11-06T05:02:50Z- handleDVL() - DVL msg epochMillisec: 1730869370.87 2024-11-06T05:02:50Z- handleDvl() - RDI_Pathfinder msg: 1730869370.87 sec; seqNo:1424; #beams:4 2024-11-06T05:02:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.80 , 39.25 , 37.01 , 45.23 2024-11-06T05:02:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.12 , -0.01 2024-11-06T05:02:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869370.88 epoch sec; seqNo:4092; 4061475.37 north; 594801.53 east; 36.69 lat; -121.94 lon; 49.40 depth; utmZone: 10 2024-11-06T05:02:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869368.44 2024-11-06T05:02:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:51Z- waiting for 1730869372.44, time is 1730869370.88 2024-11-06T05:02:51Z- handleDepth()- Depth_Keller msg: 1730869371.27 epoch sec; seqNo:4093; depth 49.44 2024-11-06T05:02:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869371.30 epoch sec; seqNo:4093; 4061475.74 north; 594801.53 east; 36.69 lat; -121.94 lon; 49.44 depth; utmZone: 10 2024-11-06T05:02:51Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:02:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869368.44 2024-11-06T05:02:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:51Z- waiting for 1730869372.44, time is 1730869371.30 2024-11-06T05:02:51Z- AHRS_M2 msg: 1730869371.65 epoch sec; seqNo:3843 2024-11-06T05:02:51Z- AHRS_M2 msg: 0.09 phi; -0.13 theta; 6.15 psi 2024-11-06T05:02:51Z- handleDepth()- Depth_Keller msg: 1730869371.65 epoch sec; seqNo:4094; depth 49.48 2024-11-06T05:02:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869371.66 epoch sec; seqNo:4094; 4061476.11 north; 594800.30 east; 36.69 lat; -121.94 lon; 49.48 depth; utmZone: 10 2024-11-06T05:02:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869368.44 2024-11-06T05:02:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:51Z- waiting for 1730869372.44, time is 1730869371.66 2024-11-06T05:02:52Z- AHRS_M2 msg: 1730869372.05 epoch sec; seqNo:3844 2024-11-06T05:02:52Z- AHRS_M2 msg: 0.09 phi; -0.13 theta; 6.15 psi 2024-11-06T05:02:52Z- handleDepth()- Depth_Keller msg: 1730869372.06 epoch sec; seqNo:4095; depth 49.51 2024-11-06T05:02:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869372.07 epoch sec; seqNo:4095; 4061476.49 north; 594800.30 east; 36.69 lat; -121.94 lon; 49.51 depth; utmZone: 10 2024-11-06T05:02:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869368.44 2024-11-06T05:02:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:52Z- waiting for 1730869372.44, time is 1730869372.07 2024-11-06T05:02:52Z- AHRS_M2 msg: 1730869372.46 epoch sec; seqNo:3845 2024-11-06T05:02:52Z- AHRS_M2 msg: 0.09 phi; -0.13 theta; 6.14 psi 2024-11-06T05:02:52Z- handleDepth()- Depth_Keller msg: 1730869372.47 epoch sec; seqNo:4096; depth 49.55 2024-11-06T05:02:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869372.48 epoch sec; seqNo:4096; 4061476.87 north; 594800.30 east; 36.69 lat; -121.94 lon; 49.55 depth; utmZone: 10 2024-11-06T05:02:52Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:52Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869368.44 2024-11-06T05:02:52Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:52Z- waiting for 1730869372.44, time is 1730869372.48 2024-11-06T05:02:52Z- LcmTrn::updateTrn() >>>> pose time 1730869372.48, meas time 1730869370.87 <<<< 2024-11-06T05:02:52Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869372.48 2024-11-06T05:02:52Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869370.87 sec, ping # 0; waiting for INS data more recent than 1730869368.44... 2024-11-06T05:02:52Z- TerrainNav::motionUpdate() - currEstimate = (4061477, 594800.30, 49.55) 2024-11-06T05:02:52Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:02:52Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:02:52Z- TNavPF::Projecting Measurements 2024-11-06T05:02:52Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:02:52Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:02:52Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:02:52Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:02:52Z- TNavFilter::4 good beams. 2024-11-06T05:02:52Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:02:52Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:02:52Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:02:52Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:02:52Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:02:52Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:02:52Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:02:52Z- TerrainMapOctree: (vehN, vehE ) = (4061475.36, 594800.80). 2024-11-06T05:02:52Z- TerrainMapOctree: Min Distance = 231.01. 2024-11-06T05:02:52Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:02:52Z- Forcing Subcloud Comparison 2024-11-06T05:02:52Z- Weighting particles with subcloud comparison 2024-11-06T05:02:52Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:52Z- meanExpectedMeasDiff: -0.159234 delta_rms_squared: 1.831581 alpha: 0.989198 2024-11-06T05:02:52Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:52Z- meanExpectedMeasDiff: -0.379743 delta_rms_squared: 1.513774 alpha: 0.986960 2024-11-06T05:02:52Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:52Z- meanExpectedMeasDiff: 0.045314 delta_rms_squared: 1.284540 alpha: 0.984669 2024-11-06T05:02:52Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:52Z- meanExpectedMeasDiff: -0.164337 delta_rms_squared: 1.989077 alpha: 0.990045 2024-11-06T05:02:52Z- Subcloud NIS: 0.162380 2024-11-06T05:02:52Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:02:52Z- TNavPF:: sumWeights = 0.617855 2024-11-06T05:02:52Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:02:52Z- TNavPF::First Term of Map Covariance Matrix is 0.48 2024-11-06T05:02:52Z- TNavPF::Calculated NIS Value : 0.02 numBeams normalized NIS: 0.00 2024-11-06T05:02:52Z- TerrainNav::Resampling particle filter... 2024-11-06T05:02:52Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869370.87 sec. 2024-11-06T05:02:52Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:02:52Z- variances([0], [2], and psi_berg) = 0.808054 0.512622 0.000000 2024-11-06T05:02:52Z- publishEstimatesDecoder() - at 1730869372.48 sec 2024-11-06T05:02:52Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:02:52Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061423.04 594803.98 57.81 2024-11-06T05:02:52Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061420.12 594804.78 58.27 lat/lon: 36.69 -121.94 2024-11-06T05:02:52Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.81 0.51 0.77 0.00 2024-11-06T05:02:52Z- AHRS_M2 msg: 1730869372.87 epoch sec; seqNo:3846 2024-11-06T05:02:52Z- AHRS_M2 msg: 0.10 phi; -0.13 theta; 6.14 psi 2024-11-06T05:02:52Z- handleDepth()- Depth_Keller msg: 1730869372.88 epoch sec; seqNo:4097; depth 49.61 2024-11-06T05:02:52Z- handleDVL() - DVL msg epochMillisec: 1730869372.94 2024-11-06T05:02:52Z- handleDvl() - RDI_Pathfinder msg: 1730869372.94 sec; seqNo:1425; #beams:4 2024-11-06T05:02:52Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.61 , 38.87 , 37.19 , 44.85 2024-11-06T05:02:52Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.13 , 0.02 2024-11-06T05:02:52Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869372.96 epoch sec; seqNo:4097; 4061477.25 north; 594800.29 east; 36.69 lat; -121.94 lon; 49.61 depth; utmZone: 10 2024-11-06T05:02:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869372.48 2024-11-06T05:02:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:53Z- waiting for 1730869376.48, time is 1730869372.96 2024-11-06T05:02:53Z- AHRS_M2 msg: 1730869373.26 epoch sec; seqNo:3847 2024-11-06T05:02:53Z- AHRS_M2 msg: 0.09 phi; -0.13 theta; 6.13 psi 2024-11-06T05:02:53Z- handleDepth()- Depth_Keller msg: 1730869373.28 epoch sec; seqNo:4098; depth 49.65 2024-11-06T05:02:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869373.29 epoch sec; seqNo:4098; 4061477.63 north; 594800.29 east; 36.69 lat; -121.94 lon; 49.65 depth; utmZone: 10 2024-11-06T05:02:53Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869372.48 2024-11-06T05:02:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:53Z- waiting for 1730869376.48, time is 1730869373.29 2024-11-06T05:02:53Z- AHRS_M2 msg: 1730869373.67 epoch sec; seqNo:3848 2024-11-06T05:02:53Z- AHRS_M2 msg: 0.09 phi; -0.12 theta; 6.13 psi 2024-11-06T05:02:53Z- handleDepth()- Depth_Keller msg: 1730869373.68 epoch sec; seqNo:4099; depth 49.70 2024-11-06T05:02:53Z- handleDVL() - DVL msg epochMillisec: 1730869373.69 2024-11-06T05:02:53Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:53Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:53Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:53Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:02:53Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:02:53Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:02:53Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:02:53Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:02:53Z- handleDvl() - RDI_Pathfinder msg: 1730869373.69 sec; seqNo:1426; #beams:0 2024-11-06T05:02:53Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:02:53Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.13 , 0.02 2024-11-06T05:02:53Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869373.70 epoch sec; seqNo:4099; 4061478.00 north; 594800.28 east; 36.69 lat; -121.94 lon; 49.70 depth; utmZone: 10 2024-11-06T05:02:53Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:53Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869372.48 2024-11-06T05:02:53Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:53Z- waiting for 1730869376.48, time is 1730869373.70 2024-11-06T05:02:54Z- AHRS_M2 msg: 1730869374.07 epoch sec; seqNo:3849 2024-11-06T05:02:54Z- AHRS_M2 msg: 0.09 phi; -0.12 theta; 6.13 psi 2024-11-06T05:02:54Z- handleDepth()- Depth_Keller msg: 1730869374.08 epoch sec; seqNo:4100; depth 49.72 2024-11-06T05:02:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869374.09 epoch sec; seqNo:4100; 4061478.38 north; 594800.28 east; 36.69 lat; -121.94 lon; 49.72 depth; utmZone: 10 2024-11-06T05:02:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869372.48 2024-11-06T05:02:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:54Z- waiting for 1730869376.48, time is 1730869374.09 2024-11-06T05:02:54Z- AHRS_M2 msg: 1730869374.48 epoch sec; seqNo:3850 2024-11-06T05:02:54Z- AHRS_M2 msg: 0.09 phi; -0.12 theta; 6.12 psi 2024-11-06T05:02:54Z- handleDepth()- Depth_Keller msg: 1730869374.48 epoch sec; seqNo:4101; depth 49.77 2024-11-06T05:02:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869374.49 epoch sec; seqNo:4101; 4061478.76 north; 594800.27 east; 36.69 lat; -121.94 lon; 49.77 depth; utmZone: 10 2024-11-06T05:02:54Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:54Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869372.48 2024-11-06T05:02:54Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:54Z- waiting for 1730869376.48, time is 1730869374.49 2024-11-06T05:02:54Z- AHRS_M2 msg: 1730869374.88 epoch sec; seqNo:3851 2024-11-06T05:02:54Z- AHRS_M2 msg: 0.09 phi; -0.12 theta; 6.12 psi 2024-11-06T05:02:54Z- handleDepth()- Depth_Keller msg: 1730869374.89 epoch sec; seqNo:4102; depth 49.83 2024-11-06T05:02:54Z- handleDVL() - DVL msg epochMillisec: 1730869374.92 2024-11-06T05:02:54Z- handleDvl() - RDI_Pathfinder msg: 1730869374.92 sec; seqNo:1427; #beams:4 2024-11-06T05:02:54Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.05 , 38.69 , 36.82 , 43.73 2024-11-06T05:02:54Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.12 , 0.01 2024-11-06T05:02:54Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869374.95 epoch sec; seqNo:4102; 4061479.14 north; 594800.27 east; 36.69 lat; -121.94 lon; 49.83 depth; utmZone: 10 2024-11-06T05:02:55Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869372.48 2024-11-06T05:02:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:55Z- waiting for 1730869376.48, time is 1730869374.95 2024-11-06T05:02:55Z- AHRS_M2 msg: 1730869375.29 epoch sec; seqNo:3852 2024-11-06T05:02:55Z- AHRS_M2 msg: 0.09 phi; -0.12 theta; 6.12 psi 2024-11-06T05:02:55Z- handleDepth()- Depth_Keller msg: 1730869375.29 epoch sec; seqNo:4103; depth 49.86 2024-11-06T05:02:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869375.33 epoch sec; seqNo:4103; 4061479.14 north; 594800.27 east; 36.69 lat; -121.94 lon; 49.86 depth; utmZone: 10 2024-11-06T05:02:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869372.48 2024-11-06T05:02:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:55Z- waiting for 1730869376.48, time is 1730869375.33 2024-11-06T05:02:55Z- AHRS_M2 msg: 1730869375.69 epoch sec; seqNo:3853 2024-11-06T05:02:55Z- AHRS_M2 msg: 0.10 phi; -0.12 theta; 6.11 psi 2024-11-06T05:02:55Z- handleDepth()- Depth_Keller msg: 1730869375.70 epoch sec; seqNo:4104; depth 49.90 2024-11-06T05:02:55Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869375.71 epoch sec; seqNo:4104; 4061479.52 north; 594800.27 east; 36.69 lat; -121.94 lon; 49.90 depth; utmZone: 10 2024-11-06T05:02:55Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:55Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869372.48 2024-11-06T05:02:55Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:55Z- waiting for 1730869376.48, time is 1730869375.71 2024-11-06T05:02:56Z- AHRS_M2 msg: 1730869376.10 epoch sec; seqNo:3854 2024-11-06T05:02:56Z- AHRS_M2 msg: 0.10 phi; -0.11 theta; 6.10 psi 2024-11-06T05:02:56Z- handleDepth()- Depth_Keller msg: 1730869376.10 epoch sec; seqNo:4105; depth 49.96 2024-11-06T05:02:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869376.11 epoch sec; seqNo:4105; 4061479.88 north; 594799.04 east; 36.69 lat; -121.94 lon; 49.96 depth; utmZone: 10 2024-11-06T05:02:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869372.48 2024-11-06T05:02:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:56Z- waiting for 1730869376.48, time is 1730869376.11 2024-11-06T05:02:56Z- AHRS_M2 msg: 1730869376.50 epoch sec; seqNo:3855 2024-11-06T05:02:56Z- AHRS_M2 msg: 0.10 phi; -0.11 theta; 6.10 psi 2024-11-06T05:02:56Z- handleDepth()- Depth_Keller msg: 1730869376.50 epoch sec; seqNo:4106; depth 50.00 2024-11-06T05:02:56Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869376.52 epoch sec; seqNo:4106; 4061480.26 north; 594799.04 east; 36.69 lat; -121.94 lon; 50.00 depth; utmZone: 10 2024-11-06T05:02:56Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:56Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869372.48 2024-11-06T05:02:56Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:56Z- waiting for 1730869376.48, time is 1730869376.52 2024-11-06T05:02:56Z- LcmTrn::updateTrn() >>>> pose time 1730869376.52, meas time 1730869374.92 <<<< 2024-11-06T05:02:56Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869376.52 2024-11-06T05:02:56Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869374.92 sec, ping # 0; waiting for INS data more recent than 1730869372.48... 2024-11-06T05:02:56Z- TerrainNav::motionUpdate() - currEstimate = (4061480, 594799.04, 50.00) 2024-11-06T05:02:56Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:02:56Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:02:56Z- TNavPF::Projecting Measurements 2024-11-06T05:02:56Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:02:56Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:02:56Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:02:56Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:02:56Z- TNavFilter::4 good beams. 2024-11-06T05:02:56Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:02:56Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:02:56Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:02:56Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:02:56Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:02:56Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:02:56Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:02:56Z- TerrainMapOctree: (vehN, vehE ) = (4061478.91, 594799.54). 2024-11-06T05:02:56Z- TerrainMapOctree: Min Distance = 234.21. 2024-11-06T05:02:56Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:02:56Z- Forcing Subcloud Comparison 2024-11-06T05:02:56Z- Weighting particles with subcloud comparison 2024-11-06T05:02:56Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:56Z- meanExpectedMeasDiff: -0.097077 delta_rms_squared: 1.497299 alpha: 0.986819 2024-11-06T05:02:56Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:56Z- meanExpectedMeasDiff: -0.307782 delta_rms_squared: 1.238049 alpha: 0.984102 2024-11-06T05:02:56Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:56Z- meanExpectedMeasDiff: 0.546882 delta_rms_squared: 1.203529 alpha: 0.983654 2024-11-06T05:02:56Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:02:56Z- meanExpectedMeasDiff: -0.488483 delta_rms_squared: 1.610652 alpha: 0.987735 2024-11-06T05:02:56Z- Subcloud NIS: 0.161315 2024-11-06T05:02:56Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:02:56Z- TNavPF:: sumWeights = 0.613427 2024-11-06T05:02:56Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:02:56Z- TNavPF::First Term of Map Covariance Matrix is 0.43 2024-11-06T05:02:56Z- TNavPF::Calculated NIS Value : 0.09 numBeams normalized NIS: 0.02 2024-11-06T05:02:56Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869374.92 sec. 2024-11-06T05:02:56Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:02:56Z- variances([0], [2], and psi_berg) = 0.796444 0.512649 0.000000 2024-11-06T05:02:56Z- publishEstimatesDecoder() - at 1730869376.52 sec 2024-11-06T05:02:56Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:02:56Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061424.09 594802.35 58.70 2024-11-06T05:02:56Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061423.51 594803.52 58.73 lat/lon: 36.69 -121.94 2024-11-06T05:02:56Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.80 0.51 0.71 0.00 2024-11-06T05:02:56Z- AHRS_M2 msg: 1730869376.92 epoch sec; seqNo:3856 2024-11-06T05:02:56Z- AHRS_M2 msg: 0.10 phi; -0.11 theta; 6.10 psi 2024-11-06T05:02:56Z- handleDepth()- Depth_Keller msg: 1730869376.94 epoch sec; seqNo:4107; depth 50.04 2024-11-06T05:02:56Z- handleDVL() - DVL msg epochMillisec: 1730869376.97 2024-11-06T05:02:56Z- handleDvl() - RDI_Pathfinder msg: 1730869376.97 sec; seqNo:1428; #beams:4 2024-11-06T05:02:56Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.05 , 38.31 , 36.63 , 43.36 2024-11-06T05:02:56Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.10 , 0.01 2024-11-06T05:02:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869376.98 epoch sec; seqNo:4107; 4061480.64 north; 594799.03 east; 36.69 lat; -121.94 lon; 50.04 depth; utmZone: 10 2024-11-06T05:02:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869376.52 2024-11-06T05:02:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:57Z- waiting for 1730869380.52, time is 1730869376.98 2024-11-06T05:02:57Z- AHRS_M2 msg: 1730869377.30 epoch sec; seqNo:3857 2024-11-06T05:02:57Z- AHRS_M2 msg: 0.10 phi; -0.11 theta; 6.09 psi 2024-11-06T05:02:57Z- handleDepth()- Depth_Keller msg: 1730869377.31 epoch sec; seqNo:4108; depth 50.09 2024-11-06T05:02:57Z- handleDVL() - DVL msg epochMillisec: 1730869377.31 2024-11-06T05:02:57Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:57Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:57Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:57Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:02:57Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:02:57Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:02:57Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:02:57Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:02:57Z- handleDvl() - RDI_Pathfinder msg: 1730869377.31 sec; seqNo:1429; #beams:0 2024-11-06T05:02:57Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:02:57Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.10 , 0.01 2024-11-06T05:02:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869377.33 epoch sec; seqNo:4108; 4061481.02 north; 594799.03 east; 36.69 lat; -121.94 lon; 50.09 depth; utmZone: 10 2024-11-06T05:02:57Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869376.52 2024-11-06T05:02:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:57Z- waiting for 1730869380.52, time is 1730869377.33 2024-11-06T05:02:57Z- AHRS_M2 msg: 1730869377.71 epoch sec; seqNo:3858 2024-11-06T05:02:57Z- AHRS_M2 msg: 0.10 phi; -0.11 theta; 6.09 psi 2024-11-06T05:02:57Z- handleDepth()- Depth_Keller msg: 1730869377.72 epoch sec; seqNo:4109; depth 50.12 2024-11-06T05:02:57Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869377.73 epoch sec; seqNo:4109; 4061481.40 north; 594799.02 east; 36.69 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T05:02:57Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:57Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869376.52 2024-11-06T05:02:57Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:57Z- waiting for 1730869380.52, time is 1730869377.73 2024-11-06T05:02:58Z- AHRS_M2 msg: 1730869378.11 epoch sec; seqNo:3859 2024-11-06T05:02:58Z- AHRS_M2 msg: 0.10 phi; -0.11 theta; 6.09 psi 2024-11-06T05:02:58Z- handleDepth()- Depth_Keller msg: 1730869378.12 epoch sec; seqNo:4110; depth 50.16 2024-11-06T05:02:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869378.14 epoch sec; seqNo:4110; 4061481.78 north; 594799.02 east; 36.69 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T05:02:58Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869376.52 2024-11-06T05:02:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:58Z- waiting for 1730869380.52, time is 1730869378.14 2024-11-06T05:02:58Z- AHRS_M2 msg: 1730869378.52 epoch sec; seqNo:3860 2024-11-06T05:02:58Z- AHRS_M2 msg: 0.10 phi; -0.11 theta; 6.08 psi 2024-11-06T05:02:58Z- handleDepth()- Depth_Keller msg: 1730869378.52 epoch sec; seqNo:4111; depth 50.20 2024-11-06T05:02:58Z- handleDVL() - DVL msg epochMillisec: 1730869378.52 2024-11-06T05:02:58Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:58Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:58Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:02:58Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:02:58Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:02:58Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:02:58Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:02:58Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:02:58Z- handleDvl() - RDI_Pathfinder msg: 1730869378.52 sec; seqNo:1430; #beams:0 2024-11-06T05:02:58Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:02:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.10 , 0.01 2024-11-06T05:02:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869378.53 epoch sec; seqNo:4111; 4061482.16 north; 594799.02 east; 36.69 lat; -121.94 lon; 50.20 depth; utmZone: 10 2024-11-06T05:02:58Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:58Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869376.52 2024-11-06T05:02:58Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:58Z- waiting for 1730869380.52, time is 1730869378.53 2024-11-06T05:02:58Z- AHRS_M2 msg: 1730869378.92 epoch sec; seqNo:3861 2024-11-06T05:02:58Z- AHRS_M2 msg: 0.10 phi; -0.11 theta; 6.08 psi 2024-11-06T05:02:58Z- handleDepth()- Depth_Keller msg: 1730869378.92 epoch sec; seqNo:4112; depth 50.25 2024-11-06T05:02:58Z- handleDVL() - DVL msg epochMillisec: 1730869378.95 2024-11-06T05:02:58Z- handleDvl() - RDI_Pathfinder msg: 1730869378.95 sec; seqNo:1431; #beams:4 2024-11-06T05:02:58Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 37.94 , 36.63 , 42.80 2024-11-06T05:02:58Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.07 , 0.02 2024-11-06T05:02:58Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869378.96 epoch sec; seqNo:4112; 4061482.54 north; 594799.01 east; 36.69 lat; -121.94 lon; 50.25 depth; utmZone: 10 2024-11-06T05:02:59Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:02:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869376.52 2024-11-06T05:02:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:59Z- waiting for 1730869380.52, time is 1730869378.96 2024-11-06T05:02:59Z- AHRS_M2 msg: 1730869379.33 epoch sec; seqNo:3862 2024-11-06T05:02:59Z- AHRS_M2 msg: 0.10 phi; -0.11 theta; 6.08 psi 2024-11-06T05:02:59Z- handleDepth()- Depth_Keller msg: 1730869379.34 epoch sec; seqNo:4113; depth 50.30 2024-11-06T05:02:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869379.37 epoch sec; seqNo:4113; 4061482.92 north; 594799.01 east; 36.69 lat; -121.94 lon; 50.30 depth; utmZone: 10 2024-11-06T05:02:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869376.52 2024-11-06T05:02:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:59Z- waiting for 1730869380.52, time is 1730869379.37 2024-11-06T05:02:59Z- AHRS_M2 msg: 1730869379.73 epoch sec; seqNo:3863 2024-11-06T05:02:59Z- AHRS_M2 msg: 0.10 phi; -0.10 theta; 6.08 psi 2024-11-06T05:02:59Z- handleDepth()- Depth_Keller msg: 1730869379.74 epoch sec; seqNo:4114; depth 50.33 2024-11-06T05:02:59Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869379.75 epoch sec; seqNo:4114; 4061483.29 north; 594799.00 east; 36.69 lat; -121.94 lon; 50.33 depth; utmZone: 10 2024-11-06T05:02:59Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:02:59Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869376.52 2024-11-06T05:02:59Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:02:59Z- waiting for 1730869380.52, time is 1730869379.75 2024-11-06T05:03:00Z- handleDepth()- Depth_Keller msg: 1730869380.15 epoch sec; seqNo:4115; depth 50.37 2024-11-06T05:03:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869380.16 epoch sec; seqNo:4115; 4061483.29 north; 594799.00 east; 36.69 lat; -121.94 lon; 50.37 depth; utmZone: 10 2024-11-06T05:03:00Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:03:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869376.52 2024-11-06T05:03:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:00Z- waiting for 1730869380.52, time is 1730869380.16 2024-11-06T05:03:00Z- AHRS_M2 msg: 1730869380.54 epoch sec; seqNo:3864 2024-11-06T05:03:00Z- AHRS_M2 msg: 0.10 phi; -0.10 theta; 6.07 psi 2024-11-06T05:03:00Z- handleDepth()- Depth_Keller msg: 1730869380.55 epoch sec; seqNo:4116; depth 50.40 2024-11-06T05:03:00Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869380.56 epoch sec; seqNo:4116; 4061483.67 north; 594799.00 east; 36.69 lat; -121.94 lon; 50.40 depth; utmZone: 10 2024-11-06T05:03:00Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:00Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869376.52 2024-11-06T05:03:00Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:00Z- waiting for 1730869380.52, time is 1730869380.56 2024-11-06T05:03:00Z- LcmTrn::updateTrn() >>>> pose time 1730869380.56, meas time 1730869378.95 <<<< 2024-11-06T05:03:00Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869380.56 2024-11-06T05:03:00Z- TerrainNav::Delayed incorporating measurement type 1 from time = 1730869378.95 sec, ping # 0; waiting for INS data more recent than 1730869376.52... 2024-11-06T05:03:00Z- TerrainNav::motionUpdate() - currEstimate = (4061484, 594799.00, 50.40) 2024-11-06T05:03:00Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:03:00Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:03:00Z- TNavPF::Projecting Measurements 2024-11-06T05:03:00Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:03:00Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:03:00Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:03:00Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:03:00Z- TNavFilter::4 good beams. 2024-11-06T05:03:00Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:03:00Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:03:00Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:03:00Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:03:00Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:03:00Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:03:00Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:03:00Z- TerrainMapOctree: (vehN, vehE ) = (4061482.31, 594799.01). 2024-11-06T05:03:00Z- TerrainMapOctree: Min Distance = 237.42. 2024-11-06T05:03:00Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:03:00Z- Forcing Subcloud Comparison 2024-11-06T05:03:00Z- Weighting particles with subcloud comparison 2024-11-06T05:03:00Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:00Z- meanExpectedMeasDiff: 1.078872 delta_rms_squared: 1.288216 alpha: 0.984712 2024-11-06T05:03:00Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:00Z- meanExpectedMeasDiff: -0.383235 delta_rms_squared: 1.150646 alpha: 0.982915 2024-11-06T05:03:00Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:00Z- meanExpectedMeasDiff: 0.554795 delta_rms_squared: 1.132838 alpha: 0.982652 2024-11-06T05:03:00Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:00Z- meanExpectedMeasDiff: -0.369912 delta_rms_squared: 1.443491 alpha: 0.986334 2024-11-06T05:03:00Z- Subcloud NIS: 0.162031 2024-11-06T05:03:00Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:03:00Z- TNavPF:: sumWeights = 0.563581 2024-11-06T05:03:00Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:03:00Z- TNavPF::First Term of Map Covariance Matrix is 0.45 2024-11-06T05:03:00Z- TNavPF::Calculated NIS Value : 0.18 numBeams normalized NIS: 0.04 2024-11-06T05:03:00Z- TerrainNav::Resampling particle filter... 2024-11-06T05:03:00Z- TerrainNav::motionUpdate - Measurement type 1 successfully incorporated from time = 1730869378.95 sec. 2024-11-06T05:03:00Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:03:00Z- variances([0], [2], and psi_berg) = 0.793877 0.524294 0.000000 2024-11-06T05:03:00Z- publishEstimatesDecoder() - at 1730869380.56 sec 2024-11-06T05:03:00Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:03:00Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061429.78 594802.64 60.61 2024-11-06T05:03:00Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061426.91 594803.51 59.04 lat/lon: 36.69 -121.94 2024-11-06T05:03:00Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.79 0.52 0.69 0.00 2024-11-06T05:03:00Z- AHRS_M2 msg: 1730869380.94 epoch sec; seqNo:3865 2024-11-06T05:03:00Z- AHRS_M2 msg: 0.11 phi; -0.09 theta; 6.07 psi 2024-11-06T05:03:00Z- handleDepth()- Depth_Keller msg: 1730869380.95 epoch sec; seqNo:4117; depth 50.43 2024-11-06T05:03:01Z- handleDVL() - DVL msg epochMillisec: 1730869380.99 2024-11-06T05:03:01Z- handleDvl() - RDI_Pathfinder msg: 1730869380.99 sec; seqNo:1432; #beams:4 2024-11-06T05:03:01Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 37.75 , 36.63 , 42.61 2024-11-06T05:03:01Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.04 , 0.02 2024-11-06T05:03:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869381.01 epoch sec; seqNo:4117; 4061484.04 north; 594797.77 east; 36.69 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T05:03:01Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869380.56 2024-11-06T05:03:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:01Z- waiting for 1730869384.56, time is 1730869381.01 2024-11-06T05:03:01Z- handleDepth()- Depth_Keller msg: 1730869381.38 epoch sec; seqNo:4118; depth 50.47 2024-11-06T05:03:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869381.40 epoch sec; seqNo:4118; 4061484.42 north; 594797.77 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T05:03:01Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:03:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869380.56 2024-11-06T05:03:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:01Z- waiting for 1730869384.56, time is 1730869381.40 2024-11-06T05:03:01Z- AHRS_M2 msg: 1730869381.75 epoch sec; seqNo:3866 2024-11-06T05:03:01Z- AHRS_M2 msg: 0.11 phi; -0.09 theta; 6.07 psi 2024-11-06T05:03:01Z- handleDepth()- Depth_Keller msg: 1730869381.76 epoch sec; seqNo:4119; depth 50.52 2024-11-06T05:03:01Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869381.77 epoch sec; seqNo:4119; 4061484.80 north; 594797.77 east; 36.69 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T05:03:01Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:01Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869380.56 2024-11-06T05:03:01Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:01Z- waiting for 1730869384.56, time is 1730869381.77 2024-11-06T05:03:02Z- AHRS_M2 msg: 1730869382.15 epoch sec; seqNo:3867 2024-11-06T05:03:02Z- AHRS_M2 msg: 0.11 phi; -0.08 theta; 6.06 psi 2024-11-06T05:03:02Z- handleDepth()- Depth_Keller msg: 1730869382.16 epoch sec; seqNo:4120; depth 50.52 2024-11-06T05:03:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869382.18 epoch sec; seqNo:4120; 4061485.18 north; 594797.76 east; 36.69 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T05:03:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869380.56 2024-11-06T05:03:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:02Z- waiting for 1730869384.56, time is 1730869382.18 2024-11-06T05:03:02Z- AHRS_M2 msg: 1730869382.56 epoch sec; seqNo:3868 2024-11-06T05:03:02Z- AHRS_M2 msg: 0.11 phi; -0.08 theta; 6.06 psi 2024-11-06T05:03:02Z- handleDepth()- Depth_Keller msg: 1730869382.57 epoch sec; seqNo:4121; depth 50.56 2024-11-06T05:03:02Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869382.58 epoch sec; seqNo:4121; 4061485.55 north; 594797.76 east; 36.69 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T05:03:02Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:02Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869380.56 2024-11-06T05:03:02Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:02Z- waiting for 1730869384.56, time is 1730869382.58 2024-11-06T05:03:02Z- AHRS_M2 msg: 1730869382.96 epoch sec; seqNo:3869 2024-11-06T05:03:02Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 6.06 psi 2024-11-06T05:03:02Z- handleDepth()- Depth_Keller msg: 1730869382.97 epoch sec; seqNo:4122; depth 50.59 2024-11-06T05:03:02Z- handleDVL() - DVL msg epochMillisec: 1730869382.99 2024-11-06T05:03:02Z- handleDvl() - RDI_Pathfinder msg: 1730869382.99 sec; seqNo:1433; #beams:4 2024-11-06T05:03:02Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 37.01 , 36.63 , 42.05 2024-11-06T05:03:02Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , -0.00 2024-11-06T05:03:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869383.00 epoch sec; seqNo:4122; 4061485.93 north; 594797.75 east; 36.69 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T05:03:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869380.56 2024-11-06T05:03:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:03Z- waiting for 1730869384.56, time is 1730869383.00 2024-11-06T05:03:03Z- AHRS_M2 msg: 1730869383.38 epoch sec; seqNo:3870 2024-11-06T05:03:03Z- AHRS_M2 msg: 0.12 phi; -0.08 theta; 6.06 psi 2024-11-06T05:03:03Z- handleDepth()- Depth_Keller msg: 1730869383.39 epoch sec; seqNo:4123; depth 50.62 2024-11-06T05:03:03Z- handleDVL() - DVL msg epochMillisec: 1730869383.39 2024-11-06T05:03:03Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:03Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:03Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:03Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:03:03Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:03:03Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:03:03Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:03:03Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:03:03Z- handleDvl() - RDI_Pathfinder msg: 1730869383.39 sec; seqNo:1434; #beams:0 2024-11-06T05:03:03Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:03:03Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , -0.00 2024-11-06T05:03:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869383.40 epoch sec; seqNo:4123; 4061486.31 north; 594797.75 east; 36.69 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T05:03:03Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869380.56 2024-11-06T05:03:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:03Z- waiting for 1730869384.56, time is 1730869383.40 2024-11-06T05:03:03Z- AHRS_M2 msg: 1730869383.77 epoch sec; seqNo:3871 2024-11-06T05:03:03Z- AHRS_M2 msg: 0.12 phi; -0.07 theta; 6.06 psi 2024-11-06T05:03:03Z- handleDepth()- Depth_Keller msg: 1730869383.80 epoch sec; seqNo:4124; depth 50.63 2024-11-06T05:03:03Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869383.82 epoch sec; seqNo:4124; 4061486.69 north; 594797.75 east; 36.69 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T05:03:03Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:03Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869380.56 2024-11-06T05:03:03Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:03Z- waiting for 1730869384.56, time is 1730869383.82 2024-11-06T05:03:04Z- AHRS_M2 msg: 1730869384.17 epoch sec; seqNo:3872 2024-11-06T05:03:04Z- AHRS_M2 msg: 0.12 phi; -0.07 theta; 6.06 psi 2024-11-06T05:03:04Z- handleDepth()- Depth_Keller msg: 1730869384.19 epoch sec; seqNo:4125; depth 50.66 2024-11-06T05:03:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869384.19 epoch sec; seqNo:4125; 4061487.07 north; 594797.74 east; 36.69 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T05:03:04Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869380.56 2024-11-06T05:03:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:04Z- waiting for 1730869384.56, time is 1730869384.19 2024-11-06T05:03:04Z- AHRS_M2 msg: 1730869384.58 epoch sec; seqNo:3873 2024-11-06T05:03:04Z- AHRS_M2 msg: 0.12 phi; -0.07 theta; 6.05 psi 2024-11-06T05:03:04Z- handleDepth()- Depth_Keller msg: 1730869384.59 epoch sec; seqNo:4126; depth 50.67 2024-11-06T05:03:04Z- handleDVL() - DVL msg epochMillisec: 1730869384.59 2024-11-06T05:03:04Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:04Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:04Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:04Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:03:04Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:03:04Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:03:04Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:03:04Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:03:04Z- handleDvl() - RDI_Pathfinder msg: 1730869384.59 sec; seqNo:1435; #beams:0 2024-11-06T05:03:04Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:03:04Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , -0.00 2024-11-06T05:03:04Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869384.60 epoch sec; seqNo:4126; 4061487.45 north; 594797.74 east; 36.69 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T05:03:04Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:04Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869380.56 2024-11-06T05:03:04Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:04Z- waiting for 1730869384.56, time is 1730869384.60 2024-11-06T05:03:04Z- LcmTrn::updateTrn() >>>> pose time 1730869384.60, meas time 1730869384.59 <<<< 2024-11-06T05:03:04Z- LcmTrn::updateTrn() equating pose 1730869384.59 and meas times 1730869384.59 2024-11-06T05:03:04Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869384.59 2024-11-06T05:03:04Z- TerrainNav::motionUpdate() - currEstimate = (4061487, 594797.74, 50.67) 2024-11-06T05:03:04Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:03:04Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:03:04Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T05:03:04Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T05:03:04Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T05:03:04Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:03:04Z- variances([0], [2], and psi_berg) = 0.796484 0.524758 0.000000 2024-11-06T05:03:04Z- publishEstimatesDecoder() - at 1730869384.59 sec 2024-11-06T05:03:04Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:03:04Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061433.52 594801.39 61.34 2024-11-06T05:03:04Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061430.68 594802.24 59.31 lat/lon: 36.69 -121.94 2024-11-06T05:03:04Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.80 0.52 0.94 0.00 2024-11-06T05:03:04Z- AHRS_M2 msg: 1730869384.98 epoch sec; seqNo:3874 2024-11-06T05:03:04Z- AHRS_M2 msg: 0.12 phi; -0.07 theta; 6.05 psi 2024-11-06T05:03:05Z- handleDepth()- Depth_Keller msg: 1730869385.00 epoch sec; seqNo:4127; depth 50.70 2024-11-06T05:03:05Z- handleDVL() - DVL msg epochMillisec: 1730869385.02 2024-11-06T05:03:05Z- handleDvl() - RDI_Pathfinder msg: 1730869385.02 sec; seqNo:1436; #beams:4 2024-11-06T05:03:05Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.87 , 36.63 , 36.44 , 41.12 2024-11-06T05:03:05Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , -0.02 2024-11-06T05:03:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869385.03 epoch sec; seqNo:4127; 4061487.83 north; 594797.73 east; 36.69 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T05:03:05Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869384.59 2024-11-06T05:03:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:05Z- waiting for 1730869388.59, time is 1730869385.03 2024-11-06T05:03:05Z- AHRS_M2 msg: 1730869385.39 epoch sec; seqNo:3875 2024-11-06T05:03:05Z- AHRS_M2 msg: 0.12 phi; -0.06 theta; 6.05 psi 2024-11-06T05:03:05Z- handleDepth()- Depth_Keller msg: 1730869385.40 epoch sec; seqNo:4128; depth 50.69 2024-11-06T05:03:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869385.43 epoch sec; seqNo:4128; 4061488.21 north; 594797.73 east; 36.69 lat; -121.94 lon; 50.69 depth; utmZone: 10 2024-11-06T05:03:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869384.59 2024-11-06T05:03:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:05Z- waiting for 1730869388.59, time is 1730869385.43 2024-11-06T05:03:05Z- AHRS_M2 msg: 1730869385.79 epoch sec; seqNo:3876 2024-11-06T05:03:05Z- AHRS_M2 msg: 0.12 phi; -0.06 theta; 6.05 psi 2024-11-06T05:03:05Z- handleDepth()- Depth_Keller msg: 1730869385.80 epoch sec; seqNo:4129; depth 50.73 2024-11-06T05:03:05Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869385.81 epoch sec; seqNo:4129; 4061488.19 north; 594796.51 east; 36.69 lat; -121.94 lon; 50.73 depth; utmZone: 10 2024-11-06T05:03:05Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:05Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869384.59 2024-11-06T05:03:05Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:05Z- waiting for 1730869388.59, time is 1730869385.81 2024-11-06T05:03:06Z- AHRS_M2 msg: 1730869386.20 epoch sec; seqNo:3877 2024-11-06T05:03:06Z- AHRS_M2 msg: 0.12 phi; -0.06 theta; 6.05 psi 2024-11-06T05:03:06Z- handleDepth()- Depth_Keller msg: 1730869386.20 epoch sec; seqNo:4130; depth 50.74 2024-11-06T05:03:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869386.21 epoch sec; seqNo:4130; 4061488.57 north; 594796.50 east; 36.69 lat; -121.94 lon; 50.74 depth; utmZone: 10 2024-11-06T05:03:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869384.59 2024-11-06T05:03:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:06Z- waiting for 1730869388.59, time is 1730869386.21 2024-11-06T05:03:06Z- AHRS_M2 msg: 1730869386.60 epoch sec; seqNo:3878 2024-11-06T05:03:06Z- AHRS_M2 msg: 0.12 phi; -0.06 theta; 6.04 psi 2024-11-06T05:03:06Z- handleDepth()- Depth_Keller msg: 1730869386.61 epoch sec; seqNo:4131; depth 50.75 2024-11-06T05:03:06Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869386.62 epoch sec; seqNo:4131; 4061488.95 north; 594796.50 east; 36.69 lat; -121.94 lon; 50.75 depth; utmZone: 10 2024-11-06T05:03:06Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:06Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869384.59 2024-11-06T05:03:06Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:06Z- waiting for 1730869388.59, time is 1730869386.62 2024-11-06T05:03:07Z- handleDepth()- Depth_Keller msg: 1730869387.02 epoch sec; seqNo:4132; depth 50.74 2024-11-06T05:03:07Z- handleDVL() - DVL msg epochMillisec: 1730869387.04 2024-11-06T05:03:07Z- handleDvl() - RDI_Pathfinder msg: 1730869387.04 sec; seqNo:1437; #beams:4 2024-11-06T05:03:07Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.49 , 36.26 , 36.26 , 40.56 2024-11-06T05:03:07Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.09 , -0.05 2024-11-06T05:03:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869387.05 epoch sec; seqNo:4132; 4061489.33 north; 594796.50 east; 36.69 lat; -121.94 lon; 50.74 depth; utmZone: 10 2024-11-06T05:03:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869384.59 2024-11-06T05:03:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:07Z- waiting for 1730869388.59, time is 1730869387.05 2024-11-06T05:03:07Z- handleDepth()- Depth_Keller msg: 1730869387.44 epoch sec; seqNo:4133; depth 50.78 2024-11-06T05:03:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869387.46 epoch sec; seqNo:4133; 4061489.71 north; 594796.49 east; 36.69 lat; -121.94 lon; 50.78 depth; utmZone: 10 2024-11-06T05:03:07Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:03:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869384.59 2024-11-06T05:03:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:07Z- waiting for 1730869388.59, time is 1730869387.46 2024-11-06T05:03:07Z- AHRS_M2 msg: 1730869387.82 epoch sec; seqNo:3879 2024-11-06T05:03:07Z- AHRS_M2 msg: 0.12 phi; -0.05 theta; 6.04 psi 2024-11-06T05:03:07Z- handleDepth()- Depth_Keller msg: 1730869387.82 epoch sec; seqNo:4134; depth 50.77 2024-11-06T05:03:07Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869387.83 epoch sec; seqNo:4134; 4061490.09 north; 594796.49 east; 36.69 lat; -121.94 lon; 50.77 depth; utmZone: 10 2024-11-06T05:03:07Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:07Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869384.59 2024-11-06T05:03:07Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:07Z- waiting for 1730869388.59, time is 1730869387.83 2024-11-06T05:03:08Z- AHRS_M2 msg: 1730869388.21 epoch sec; seqNo:3880 2024-11-06T05:03:08Z- AHRS_M2 msg: 0.12 phi; -0.05 theta; 6.04 psi 2024-11-06T05:03:08Z- handleDepth()- Depth_Keller msg: 1730869388.24 epoch sec; seqNo:4135; depth 50.80 2024-11-06T05:03:08Z- handleDVL() - DVL msg epochMillisec: 1730869388.24 2024-11-06T05:03:08Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:08Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:08Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:08Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:03:08Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:03:08Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:03:08Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:03:08Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:03:08Z- handleDvl() - RDI_Pathfinder msg: 1730869388.24 sec; seqNo:1438; #beams:0 2024-11-06T05:03:08Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:03:08Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.09 , -0.05 2024-11-06T05:03:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869388.25 epoch sec; seqNo:4135; 4061490.47 north; 594796.48 east; 36.69 lat; -121.94 lon; 50.80 depth; utmZone: 10 2024-11-06T05:03:08Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869384.59 2024-11-06T05:03:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:08Z- waiting for 1730869388.59, time is 1730869388.25 2024-11-06T05:03:08Z- AHRS_M2 msg: 1730869388.62 epoch sec; seqNo:3881 2024-11-06T05:03:08Z- AHRS_M2 msg: 0.12 phi; -0.05 theta; 6.03 psi 2024-11-06T05:03:08Z- handleDepth()- Depth_Keller msg: 1730869388.62 epoch sec; seqNo:4136; depth 50.82 2024-11-06T05:03:08Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869388.63 epoch sec; seqNo:4136; 4061490.84 north; 594796.48 east; 36.69 lat; -121.94 lon; 50.82 depth; utmZone: 10 2024-11-06T05:03:08Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:08Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869384.59 2024-11-06T05:03:08Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:08Z- waiting for 1730869388.59, time is 1730869388.63 2024-11-06T05:03:08Z- LcmTrn::updateTrn() >>>> pose time 1730869388.63, meas time 1730869388.24 <<<< 2024-11-06T05:03:08Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869388.63 2024-11-06T05:03:08Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T05:03:08Z- TerrainNav::motionUpdate() - currEstimate = (4061491, 594796.48, 50.82) 2024-11-06T05:03:08Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:03:08Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:03:08Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T05:03:08Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T05:03:08Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:03:08Z- variances([0], [2], and psi_berg) = 0.798574 0.527901 0.000000 2024-11-06T05:03:08Z- publishEstimatesDecoder() - at 1730869388.63 sec 2024-11-06T05:03:08Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:03:08Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061436.82 594800.08 62.09 2024-11-06T05:03:08Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061434.08 594800.98 59.45 lat/lon: 36.69 -121.94 2024-11-06T05:03:08Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.80 0.53 1.16 0.00 2024-11-06T05:03:09Z- handleDepth()- Depth_Keller msg: 1730869389.04 epoch sec; seqNo:4137; depth 50.83 2024-11-06T05:03:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869389.06 epoch sec; seqNo:4137; 4061491.22 north; 594796.47 east; 36.69 lat; -121.94 lon; 50.83 depth; utmZone: 10 2024-11-06T05:03:09Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:03:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869388.63 2024-11-06T05:03:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:09Z- waiting for 1730869392.63, time is 1730869389.06 2024-11-06T05:03:09Z- AHRS_M2 msg: 1730869389.43 epoch sec; seqNo:3882 2024-11-06T05:03:09Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 6.03 psi 2024-11-06T05:03:09Z- handleDepth()- Depth_Keller msg: 1730869389.44 epoch sec; seqNo:4138; depth 50.85 2024-11-06T05:03:09Z- handleDVL() - DVL msg epochMillisec: 1730869389.45 2024-11-06T05:03:09Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:09Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:09Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:09Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:03:09Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:03:09Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:03:09Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:03:09Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:03:09Z- handleDvl() - RDI_Pathfinder msg: 1730869389.45 sec; seqNo:1439; #beams:0 2024-11-06T05:03:09Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:03:09Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.09 , -0.05 2024-11-06T05:03:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869389.47 epoch sec; seqNo:4138; 4061491.60 north; 594796.47 east; 36.69 lat; -121.94 lon; 50.85 depth; utmZone: 10 2024-11-06T05:03:09Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869388.63 2024-11-06T05:03:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:09Z- waiting for 1730869392.63, time is 1730869389.47 2024-11-06T05:03:09Z- AHRS_M2 msg: 1730869389.83 epoch sec; seqNo:3883 2024-11-06T05:03:09Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 6.03 psi 2024-11-06T05:03:09Z- handleDepth()- Depth_Keller msg: 1730869389.84 epoch sec; seqNo:4139; depth 50.86 2024-11-06T05:03:09Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869389.85 epoch sec; seqNo:4139; 4061491.60 north; 594796.47 east; 36.69 lat; -121.94 lon; 50.86 depth; utmZone: 10 2024-11-06T05:03:09Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:09Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869388.63 2024-11-06T05:03:09Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:09Z- waiting for 1730869392.63, time is 1730869389.85 2024-11-06T05:03:10Z- AHRS_M2 msg: 1730869390.23 epoch sec; seqNo:3884 2024-11-06T05:03:10Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 6.02 psi 2024-11-06T05:03:10Z- handleDepth()- Depth_Keller msg: 1730869390.24 epoch sec; seqNo:4140; depth 50.89 2024-11-06T05:03:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869390.25 epoch sec; seqNo:4140; 4061491.97 north; 594795.25 east; 36.69 lat; -121.94 lon; 50.89 depth; utmZone: 10 2024-11-06T05:03:10Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869388.63 2024-11-06T05:03:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:10Z- waiting for 1730869392.63, time is 1730869390.25 2024-11-06T05:03:10Z- AHRS_M2 msg: 1730869390.64 epoch sec; seqNo:3885 2024-11-06T05:03:10Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 6.02 psi 2024-11-06T05:03:10Z- handleDepth()- Depth_Keller msg: 1730869390.64 epoch sec; seqNo:4141; depth 50.90 2024-11-06T05:03:10Z- handleDVL() - DVL msg epochMillisec: 1730869390.65 2024-11-06T05:03:10Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:10Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:10Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:10Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:03:10Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:03:10Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:03:10Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:03:10Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:03:10Z- handleDvl() - RDI_Pathfinder msg: 1730869390.65 sec; seqNo:1440; #beams:0 2024-11-06T05:03:10Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:03:10Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.09 , -0.05 2024-11-06T05:03:10Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869390.65 epoch sec; seqNo:4141; 4061492.35 north; 594795.24 east; 36.69 lat; -121.94 lon; 50.90 depth; utmZone: 10 2024-11-06T05:03:10Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:10Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869388.63 2024-11-06T05:03:10Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:10Z- waiting for 1730869392.63, time is 1730869390.65 2024-11-06T05:03:11Z- AHRS_M2 msg: 1730869391.04 epoch sec; seqNo:3886 2024-11-06T05:03:11Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 6.02 psi 2024-11-06T05:03:11Z- handleDepth()- Depth_Keller msg: 1730869391.05 epoch sec; seqNo:4142; depth 50.92 2024-11-06T05:03:11Z- handleDVL() - DVL msg epochMillisec: 1730869391.07 2024-11-06T05:03:11Z- handleDvl() - RDI_Pathfinder msg: 1730869391.07 sec; seqNo:1441; #beams:4 2024-11-06T05:03:11Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.30 , 35.32 , 34.95 , 40.00 2024-11-06T05:03:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.14 , -0.02 2024-11-06T05:03:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869391.10 epoch sec; seqNo:4142; 4061492.73 north; 594795.24 east; 36.69 lat; -121.94 lon; 50.92 depth; utmZone: 10 2024-11-06T05:03:11Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869388.63 2024-11-06T05:03:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:11Z- waiting for 1730869392.63, time is 1730869391.10 2024-11-06T05:03:11Z- AHRS_M2 msg: 1730869391.48 epoch sec; seqNo:3887 2024-11-06T05:03:11Z- AHRS_M2 msg: 0.12 phi; -0.04 theta; 6.01 psi 2024-11-06T05:03:11Z- handleDepth()- Depth_Keller msg: 1730869391.49 epoch sec; seqNo:4143; depth 50.94 2024-11-06T05:03:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869391.52 epoch sec; seqNo:4143; 4061493.10 north; 594795.23 east; 36.69 lat; -121.94 lon; 50.94 depth; utmZone: 10 2024-11-06T05:03:11Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:11Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869388.63 2024-11-06T05:03:11Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:11Z- waiting for 1730869392.63, time is 1730869391.52 2024-11-06T05:03:11Z- handleDepth()- Depth_Keller msg: 1730869391.87 epoch sec; seqNo:4144; depth 50.97 2024-11-06T05:03:11Z- handleDVL() - DVL msg epochMillisec: 1730869391.88 2024-11-06T05:03:11Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:11Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:11Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:11Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:03:11Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:03:11Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:03:11Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:03:11Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:03:11Z- handleDvl() - RDI_Pathfinder msg: 1730869391.88 sec; seqNo:1442; #beams:0 2024-11-06T05:03:11Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:03:11Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.14 , -0.02 2024-11-06T05:03:11Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869391.89 epoch sec; seqNo:4144; 4061493.48 north; 594795.23 east; 36.69 lat; -121.94 lon; 50.97 depth; utmZone: 10 2024-11-06T05:03:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869388.63 2024-11-06T05:03:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:12Z- waiting for 1730869392.63, time is 1730869391.89 2024-11-06T05:03:12Z- AHRS_M2 msg: 1730869392.26 epoch sec; seqNo:3888 2024-11-06T05:03:12Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 6.01 psi 2024-11-06T05:03:12Z- handleDepth()- Depth_Keller msg: 1730869392.27 epoch sec; seqNo:4145; depth 50.98 2024-11-06T05:03:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869392.28 epoch sec; seqNo:4145; 4061493.86 north; 594795.23 east; 36.69 lat; -121.94 lon; 50.98 depth; utmZone: 10 2024-11-06T05:03:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869388.63 2024-11-06T05:03:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:12Z- waiting for 1730869392.63, time is 1730869392.28 2024-11-06T05:03:12Z- AHRS_M2 msg: 1730869392.67 epoch sec; seqNo:3889 2024-11-06T05:03:12Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 6.00 psi 2024-11-06T05:03:12Z- handleDepth()- Depth_Keller msg: 1730869392.67 epoch sec; seqNo:4146; depth 51.01 2024-11-06T05:03:12Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869392.69 epoch sec; seqNo:4146; 4061494.24 north; 594795.22 east; 36.69 lat; -121.94 lon; 51.01 depth; utmZone: 10 2024-11-06T05:03:12Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:12Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869388.63 2024-11-06T05:03:12Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:12Z- waiting for 1730869392.63, time is 1730869392.69 2024-11-06T05:03:12Z- LcmTrn::updateTrn() >>>> pose time 1730869392.69, meas time 1730869391.88 <<<< 2024-11-06T05:03:12Z- LcmTrn::updateTrn() measUpdate first last update time: 1730869392.69 2024-11-06T05:03:12Z- LcmTrn::updateTrn() measUpdate skipped due to invalid DVL 2024-11-06T05:03:12Z- TerrainNav::motionUpdate() - currEstimate = (4061494, 594795.22, 51.01) 2024-11-06T05:03:12Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:03:12Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:03:12Z- TerrainNav::motionUpdate() - Invalid velocity 2024-11-06T05:03:12Z- TerrainNav::motionUpdate() - Invalid velocity, using prior value 2024-11-06T05:03:12Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:03:12Z- variances([0], [2], and psi_berg) = 0.799180 0.530562 0.000000 2024-11-06T05:03:12Z- publishEstimatesDecoder() - at 1730869392.69 sec 2024-11-06T05:03:12Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:03:12Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061440.21 594798.77 61.52 2024-11-06T05:03:12Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061437.47 594799.73 59.64 lat/lon: 36.69 -121.94 2024-11-06T05:03:12Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.80 0.53 1.40 0.00 2024-11-06T05:03:13Z- AHRS_M2 msg: 1730869393.07 epoch sec; seqNo:3890 2024-11-06T05:03:13Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 6.00 psi 2024-11-06T05:03:13Z- handleDepth()- Depth_Keller msg: 1730869393.07 epoch sec; seqNo:4147; depth 51.04 2024-11-06T05:03:13Z- handleDVL() - DVL msg epochMillisec: 1730869393.10 2024-11-06T05:03:13Z- handleDvl() - RDI_Pathfinder msg: 1730869393.10 sec; seqNo:1443; #beams:4 2024-11-06T05:03:13Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.29 , 35.17 , 34.49 , 39.76 2024-11-06T05:03:13Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.10 , 0.04 2024-11-06T05:03:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869393.11 epoch sec; seqNo:4147; 4061494.62 north; 594795.22 east; 36.69 lat; -121.94 lon; 51.04 depth; utmZone: 10 2024-11-06T05:03:13Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869392.69 2024-11-06T05:03:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:13Z- waiting for 1730869396.69, time is 1730869393.11 2024-11-06T05:03:13Z- AHRS_M2 msg: 1730869393.47 epoch sec; seqNo:3891 2024-11-06T05:03:13Z- AHRS_M2 msg: 0.12 phi; -0.03 theta; 6.00 psi 2024-11-06T05:03:13Z- handleDepth()- Depth_Keller msg: 1730869393.47 epoch sec; seqNo:4148; depth 51.06 2024-11-06T05:03:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869393.49 epoch sec; seqNo:4148; 4061494.98 north; 594793.99 east; 36.69 lat; -121.94 lon; 51.06 depth; utmZone: 10 2024-11-06T05:03:13Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:13Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869392.69 2024-11-06T05:03:13Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:13Z- waiting for 1730869396.69, time is 1730869393.49 2024-11-06T05:03:13Z- AHRS_M2 msg: 1730869393.88 epoch sec; seqNo:3892 2024-11-06T05:03:13Z- AHRS_M2 msg: 0.13 phi; -0.03 theta; 6.00 psi 2024-11-06T05:03:13Z- handleDepth()- Depth_Keller msg: 1730869393.88 epoch sec; seqNo:4149; depth 51.09 2024-11-06T05:03:13Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869393.89 epoch sec; seqNo:4149; 4061495.36 north; 594793.99 east; 36.69 lat; -121.94 lon; 51.09 depth; utmZone: 10 2024-11-06T05:03:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869392.69 2024-11-06T05:03:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:14Z- waiting for 1730869396.69, time is 1730869393.89 2024-11-06T05:03:14Z- AHRS_M2 msg: 1730869394.28 epoch sec; seqNo:3893 2024-11-06T05:03:14Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.00 psi 2024-11-06T05:03:14Z- handleDepth()- Depth_Keller msg: 1730869394.29 epoch sec; seqNo:4150; depth 51.10 2024-11-06T05:03:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869394.30 epoch sec; seqNo:4150; 4061495.74 north; 594793.98 east; 36.69 lat; -121.94 lon; 51.10 depth; utmZone: 10 2024-11-06T05:03:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869392.69 2024-11-06T05:03:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:14Z- waiting for 1730869396.69, time is 1730869394.30 2024-11-06T05:03:14Z- AHRS_M2 msg: 1730869394.69 epoch sec; seqNo:3894 2024-11-06T05:03:14Z- AHRS_M2 msg: 0.14 phi; -0.03 theta; 6.01 psi 2024-11-06T05:03:14Z- handleDepth()- Depth_Keller msg: 1730869394.71 epoch sec; seqNo:4151; depth 51.13 2024-11-06T05:03:14Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869394.75 epoch sec; seqNo:4151; 4061496.12 north; 594793.98 east; 36.69 lat; -121.94 lon; 51.13 depth; utmZone: 10 2024-11-06T05:03:14Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:14Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869392.69 2024-11-06T05:03:14Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:14Z- waiting for 1730869396.69, time is 1730869394.75 2024-11-06T05:03:15Z- AHRS_M2 msg: 1730869395.09 epoch sec; seqNo:3895 2024-11-06T05:03:15Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.01 psi 2024-11-06T05:03:15Z- handleDepth()- Depth_Keller msg: 1730869395.09 epoch sec; seqNo:4152; depth 51.14 2024-11-06T05:03:15Z- handleDVL() - DVL msg epochMillisec: 1730869395.12 2024-11-06T05:03:15Z- handleDvl() - RDI_Pathfinder msg: 1730869395.12 sec; seqNo:1444; #beams:4 2024-11-06T05:03:15Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.80 , 34.83 , 34.49 , 39.76 2024-11-06T05:03:15Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.03 , 0.08 2024-11-06T05:03:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869395.13 epoch sec; seqNo:4152; 4061496.50 north; 594793.98 east; 36.69 lat; -121.94 lon; 51.14 depth; utmZone: 10 2024-11-06T05:03:15Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869392.69 2024-11-06T05:03:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:15Z- waiting for 1730869396.69, time is 1730869395.13 2024-11-06T05:03:15Z- AHRS_M2 msg: 1730869395.49 epoch sec; seqNo:3896 2024-11-06T05:03:15Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.01 psi 2024-11-06T05:03:15Z- handleDepth()- Depth_Keller msg: 1730869395.50 epoch sec; seqNo:4153; depth 51.16 2024-11-06T05:03:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869395.51 epoch sec; seqNo:4153; 4061496.50 north; 594793.98 east; 36.69 lat; -121.94 lon; 51.16 depth; utmZone: 10 2024-11-06T05:03:15Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:15Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869392.69 2024-11-06T05:03:15Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:15Z- waiting for 1730869396.69, time is 1730869395.51 2024-11-06T05:03:15Z- AHRS_M2 msg: 1730869395.89 epoch sec; seqNo:3897 2024-11-06T05:03:15Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.02 psi 2024-11-06T05:03:15Z- handleDepth()- Depth_Keller msg: 1730869395.91 epoch sec; seqNo:4154; depth 51.17 2024-11-06T05:03:15Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869395.92 epoch sec; seqNo:4154; 4061496.88 north; 594793.97 east; 36.69 lat; -121.94 lon; 51.17 depth; utmZone: 10 2024-11-06T05:03:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869392.69 2024-11-06T05:03:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:16Z- waiting for 1730869396.69, time is 1730869395.92 2024-11-06T05:03:16Z- AHRS_M2 msg: 1730869396.30 epoch sec; seqNo:3898 2024-11-06T05:03:16Z- AHRS_M2 msg: 0.15 phi; -0.03 theta; 6.02 psi 2024-11-06T05:03:16Z- handleDepth()- Depth_Keller msg: 1730869396.31 epoch sec; seqNo:4155; depth 51.18 2024-11-06T05:03:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869396.33 epoch sec; seqNo:4155; 4061497.26 north; 594793.97 east; 36.69 lat; -121.94 lon; 51.18 depth; utmZone: 10 2024-11-06T05:03:16Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869392.69 2024-11-06T05:03:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:16Z- waiting for 1730869396.69, time is 1730869396.33 2024-11-06T05:03:16Z- AHRS_M2 msg: 1730869396.74 epoch sec; seqNo:3899 2024-11-06T05:03:16Z- AHRS_M2 msg: 0.16 phi; -0.03 theta; 6.02 psi 2024-11-06T05:03:16Z- handleDepth()- Depth_Keller msg: 1730869396.75 epoch sec; seqNo:4156; depth 51.18 2024-11-06T05:03:16Z- handleDVL() - DVL msg epochMillisec: 1730869396.77 2024-11-06T05:03:16Z- handleDvl() - RDI_Pathfinder msg: 1730869396.77 sec; seqNo:1445; #beams:4 2024-11-06T05:03:16Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.65 , 33.98 , 34.15 , 39.76 2024-11-06T05:03:16Z- handleDvl() - RDI_Pathfinder msg: velocities 0.84 , 0.04 , 0.02 2024-11-06T05:03:16Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869396.78 epoch sec; seqNo:4156; 4061497.64 north; 594793.96 east; 36.69 lat; -121.94 lon; 51.18 depth; utmZone: 10 2024-11-06T05:03:16Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:16Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869392.69 2024-11-06T05:03:16Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:16Z- waiting for 1730869396.69, time is 1730869396.78 2024-11-06T05:03:16Z- LcmTrn::updateTrn() >>>> pose time 1730869396.78, meas time 1730869396.77 <<<< 2024-11-06T05:03:16Z- LcmTrn::updateTrn() equating pose 1730869396.77 and meas times 1730869396.77 2024-11-06T05:03:16Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869396.77 2024-11-06T05:03:16Z- TerrainNav::motionUpdate() - currEstimate = (4061498, 594793.96, 51.18) 2024-11-06T05:03:16Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:03:16Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:03:16Z- TNavPF::Projecting Measurements 2024-11-06T05:03:16Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:03:16Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:03:16Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:03:16Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:03:16Z- TNavFilter::4 good beams. 2024-11-06T05:03:16Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:03:16Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:03:16Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:03:16Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:03:16Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:03:16Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:03:16Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:03:16Z- TerrainMapOctree: (vehN, vehE ) = (4061497.64, 594793.96). 2024-11-06T05:03:16Z- TerrainMapOctree: Min Distance = 251.51. 2024-11-06T05:03:16Z- TerrainMapOctree: Using tile 2. 2024-11-06T05:03:17Z- Forcing Subcloud Comparison 2024-11-06T05:03:17Z- Weighting particles with subcloud comparison 2024-11-06T05:03:17Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:17Z- meanExpectedMeasDiff: 1.602026 delta_rms_squared: 2.665246 alpha: 0.992552 2024-11-06T05:03:17Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:17Z- meanExpectedMeasDiff: -1.320864 delta_rms_squared: 1.891242 alpha: 0.989536 2024-11-06T05:03:17Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:17Z- meanExpectedMeasDiff: -0.522074 delta_rms_squared: 1.772245 alpha: 0.988841 2024-11-06T05:03:17Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:17Z- meanExpectedMeasDiff: 0.672253 delta_rms_squared: 2.279541 alpha: 0.991303 2024-11-06T05:03:17Z- Subcloud NIS: 0.160909 2024-11-06T05:03:17Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:03:17Z- TNavPF:: sumWeights = 0.311288 2024-11-06T05:03:17Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:03:17Z- TNavPF::First Term of Map Covariance Matrix is 0.61 2024-11-06T05:03:17Z- TNavPF::Calculated NIS Value : 0.16 numBeams normalized NIS: 0.04 2024-11-06T05:03:17Z- TerrainNav::Resampling particle filter... 2024-11-06T05:03:17Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869396.77 sec, ping # 0. 2024-11-06T05:03:17Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:03:17Z- variances([0], [2], and psi_berg) = 0.760717 0.523598 0.000000 2024-11-06T05:03:17Z- publishEstimatesDecoder() - at 1730869396.77 sec 2024-11-06T05:03:17Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:03:17Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061443.78 594797.68 58.72 2024-11-06T05:03:17Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061440.88 594798.47 59.76 lat/lon: 36.69 -121.94 2024-11-06T05:03:17Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.76 0.52 0.51 0.00 2024-11-06T05:03:17Z- AHRS_M2 msg: 1730869397.13 epoch sec; seqNo:3900 2024-11-06T05:03:17Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 6.02 psi 2024-11-06T05:03:17Z- handleDepth()- Depth_Keller msg: 1730869397.13 epoch sec; seqNo:4157; depth 51.17 2024-11-06T05:03:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869397.16 epoch sec; seqNo:4157; 4061498.02 north; 594793.96 east; 36.69 lat; -121.94 lon; 51.17 depth; utmZone: 10 2024-11-06T05:03:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869396.77 2024-11-06T05:03:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:17Z- waiting for 1730869400.77, time is 1730869397.16 2024-11-06T05:03:17Z- AHRS_M2 msg: 1730869397.51 epoch sec; seqNo:3901 2024-11-06T05:03:17Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 6.03 psi 2024-11-06T05:03:17Z- handleDepth()- Depth_Keller msg: 1730869397.51 epoch sec; seqNo:4158; depth 51.18 2024-11-06T05:03:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869397.53 epoch sec; seqNo:4158; 4061498.39 north; 594793.95 east; 36.69 lat; -121.94 lon; 51.18 depth; utmZone: 10 2024-11-06T05:03:17Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:17Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869396.77 2024-11-06T05:03:17Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:17Z- waiting for 1730869400.77, time is 1730869397.53 2024-11-06T05:03:17Z- AHRS_M2 msg: 1730869397.91 epoch sec; seqNo:3902 2024-11-06T05:03:17Z- AHRS_M2 msg: 0.16 phi; -0.02 theta; 6.03 psi 2024-11-06T05:03:17Z- handleDepth()- Depth_Keller msg: 1730869397.92 epoch sec; seqNo:4159; depth 51.19 2024-11-06T05:03:17Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869397.92 epoch sec; seqNo:4159; 4061498.76 north; 594792.73 east; 36.69 lat; -121.94 lon; 51.19 depth; utmZone: 10 2024-11-06T05:03:18Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869396.77 2024-11-06T05:03:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:18Z- waiting for 1730869400.77, time is 1730869397.92 2024-11-06T05:03:18Z- handleDepth()- Depth_Keller msg: 1730869398.33 epoch sec; seqNo:4160; depth 51.18 2024-11-06T05:03:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869398.34 epoch sec; seqNo:4160; 4061499.14 north; 594792.73 east; 36.69 lat; -121.94 lon; 51.18 depth; utmZone: 10 2024-11-06T05:03:18Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:03:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869396.77 2024-11-06T05:03:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:18Z- waiting for 1730869400.77, time is 1730869398.34 2024-11-06T05:03:18Z- AHRS_M2 msg: 1730869398.75 epoch sec; seqNo:3903 2024-11-06T05:03:18Z- AHRS_M2 msg: 0.15 phi; -0.02 theta; 6.03 psi 2024-11-06T05:03:18Z- handleDepth()- Depth_Keller msg: 1730869398.75 epoch sec; seqNo:4161; depth 51.18 2024-11-06T05:03:18Z- handleDVL() - DVL msg epochMillisec: 1730869398.78 2024-11-06T05:03:18Z- handleDvl() - RDI_Pathfinder msg: 1730869398.78 sec; seqNo:1446; #beams:4 2024-11-06T05:03:18Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 42.31 , 33.81 , 33.30 , 39.42 2024-11-06T05:03:18Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , -0.07 2024-11-06T05:03:18Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869398.79 epoch sec; seqNo:4161; 4061499.52 north; 594792.72 east; 36.69 lat; -121.94 lon; 51.18 depth; utmZone: 10 2024-11-06T05:03:18Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:18Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869396.77 2024-11-06T05:03:18Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:18Z- waiting for 1730869400.77, time is 1730869398.79 2024-11-06T05:03:19Z- AHRS_M2 msg: 1730869399.12 epoch sec; seqNo:3904 2024-11-06T05:03:19Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.03 psi 2024-11-06T05:03:19Z- handleDepth()- Depth_Keller msg: 1730869399.13 epoch sec; seqNo:4162; depth 51.17 2024-11-06T05:03:19Z- handleDVL() - DVL msg epochMillisec: 1730869399.13 2024-11-06T05:03:19Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:19Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:19Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:19Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:03:19Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:03:19Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:03:19Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:03:19Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:03:19Z- handleDvl() - RDI_Pathfinder msg: 1730869399.13 sec; seqNo:1447; #beams:0 2024-11-06T05:03:19Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:03:19Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , -0.07 2024-11-06T05:03:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869399.15 epoch sec; seqNo:4162; 4061499.52 north; 594792.72 east; 36.69 lat; -121.94 lon; 51.17 depth; utmZone: 10 2024-11-06T05:03:19Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869396.77 2024-11-06T05:03:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:19Z- waiting for 1730869400.77, time is 1730869399.15 2024-11-06T05:03:19Z- AHRS_M2 msg: 1730869399.53 epoch sec; seqNo:3905 2024-11-06T05:03:19Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.03 psi 2024-11-06T05:03:19Z- handleDepth()- Depth_Keller msg: 1730869399.54 epoch sec; seqNo:4163; depth 51.17 2024-11-06T05:03:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869399.55 epoch sec; seqNo:4163; 4061499.90 north; 594792.72 east; 36.69 lat; -121.94 lon; 51.17 depth; utmZone: 10 2024-11-06T05:03:19Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:19Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869396.77 2024-11-06T05:03:19Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:19Z- waiting for 1730869400.77, time is 1730869399.55 2024-11-06T05:03:19Z- AHRS_M2 msg: 1730869399.93 epoch sec; seqNo:3906 2024-11-06T05:03:19Z- AHRS_M2 msg: 0.15 phi; -0.01 theta; 6.03 psi 2024-11-06T05:03:19Z- handleDepth()- Depth_Keller msg: 1730869399.94 epoch sec; seqNo:4164; depth 51.15 2024-11-06T05:03:19Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869399.94 epoch sec; seqNo:4164; 4061500.27 north; 594792.71 east; 36.69 lat; -121.94 lon; 51.15 depth; utmZone: 10 2024-11-06T05:03:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869396.77 2024-11-06T05:03:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:20Z- waiting for 1730869400.77, time is 1730869399.94 2024-11-06T05:03:20Z- AHRS_M2 msg: 1730869400.34 epoch sec; seqNo:3907 2024-11-06T05:03:20Z- AHRS_M2 msg: 0.14 phi; -0.00 theta; 6.03 psi 2024-11-06T05:03:20Z- handleDepth()- Depth_Keller msg: 1730869400.34 epoch sec; seqNo:4165; depth 51.15 2024-11-06T05:03:20Z- handleDVL() - DVL msg epochMillisec: 1730869400.35 2024-11-06T05:03:20Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:20Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:20Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:20Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:03:20Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:03:20Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:03:20Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:03:20Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:03:20Z- handleDvl() - RDI_Pathfinder msg: 1730869400.35 sec; seqNo:1448; #beams:0 2024-11-06T05:03:20Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:03:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.02 , -0.07 2024-11-06T05:03:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869400.35 epoch sec; seqNo:4165; 4061500.65 north; 594792.71 east; 36.69 lat; -121.94 lon; 51.15 depth; utmZone: 10 2024-11-06T05:03:20Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869396.77 2024-11-06T05:03:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:20Z- waiting for 1730869400.77, time is 1730869400.35 2024-11-06T05:03:20Z- handleDepth()- Depth_Keller msg: 1730869400.78 epoch sec; seqNo:4166; depth 51.14 2024-11-06T05:03:20Z- handleDVL() - DVL msg epochMillisec: 1730869400.80 2024-11-06T05:03:20Z- handleDvl() - RDI_Pathfinder msg: 1730869400.80 sec; seqNo:1449; #beams:4 2024-11-06T05:03:20Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.97 , 33.64 , 32.62 , 38.74 2024-11-06T05:03:20Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.08 , -0.11 2024-11-06T05:03:20Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869400.85 epoch sec; seqNo:4166; 4061501.03 north; 594792.70 east; 36.69 lat; -121.94 lon; 51.14 depth; utmZone: 10 2024-11-06T05:03:20Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:20Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869396.77 2024-11-06T05:03:20Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:20Z- waiting for 1730869400.77, time is 1730869400.85 2024-11-06T05:03:20Z- LcmTrn::updateTrn() >>>> pose time 1730869400.85, meas time 1730869400.80 <<<< 2024-11-06T05:03:20Z- LcmTrn::updateTrn() equating pose 1730869400.80 and meas times 1730869400.80 2024-11-06T05:03:20Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869400.80 2024-11-06T05:03:20Z- TerrainNav::motionUpdate() - currEstimate = (4061501, 594792.70, 51.14) 2024-11-06T05:03:20Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:03:20Z- TerrainNav:: SubcloudNIS is 0.16, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:03:20Z- TNavPF::Projecting Measurements 2024-11-06T05:03:20Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:03:20Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:03:20Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:03:20Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:03:20Z- TNavFilter::4 good beams. 2024-11-06T05:03:20Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:03:20Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:03:20Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:03:20Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:03:20Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:03:20Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:03:20Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:03:20Z- TerrainMapOctree: (vehN, vehE ) = (4061501.03, 594792.70). 2024-11-06T05:03:20Z- TerrainMapOctree: Min Distance = 252.60. 2024-11-06T05:03:20Z- TerrainMapOctree: Using tile 5. 2024-11-06T05:03:21Z- TerrainMapOctree: Switching to tile 5. 2024-11-06T05:03:22Z- TerrainMapOctree::Octree tile load /home/tethysadmin/Downloads/trnmaps/PortTiles/PortugueseLedgeTile5.bo took 1.192473 seconds. 2024-11-06T05:03:22Z- Forcing Subcloud Comparison 2024-11-06T05:03:22Z- Weighting particles with subcloud comparison 2024-11-06T05:03:22Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:22Z- meanExpectedMeasDiff: 1.970791 delta_rms_squared: 1.149778 alpha: 0.982903 2024-11-06T05:03:22Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:22Z- meanExpectedMeasDiff: -1.549948 delta_rms_squared: 1.315003 alpha: 0.985019 2024-11-06T05:03:22Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:22Z- meanExpectedMeasDiff: -1.996917 delta_rms_squared: 0.917428 alpha: 0.978665 2024-11-06T05:03:22Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:22Z- meanExpectedMeasDiff: 0.188799 delta_rms_squared: 1.030694 alpha: 0.980965 2024-11-06T05:03:22Z- Subcloud NIS: 0.170022 2024-11-06T05:03:22Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:03:22Z- TNavPF:: sumWeights = 0.194074 2024-11-06T05:03:22Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:03:22Z- TNavPF::First Term of Map Covariance Matrix is 0.70 2024-11-06T05:03:22Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.03 2024-11-06T05:03:22Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869400.80 sec, ping # 0. 2024-11-06T05:03:22Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:03:22Z- variances([0], [2], and psi_berg) = 0.842530 0.525461 0.000000 2024-11-06T05:03:22Z- publishEstimatesDecoder() - at 1730869400.80 sec 2024-11-06T05:03:22Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:03:22Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061445.44 594799.09 59.95 2024-11-06T05:03:22Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061444.29 594797.24 59.94 lat/lon: 36.69 -121.94 2024-11-06T05:03:22Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.84 0.53 0.41 0.00 2024-11-06T05:03:22Z- AHRS_M2 msg: 1730869401.14 epoch sec; seqNo:3908 2024-11-06T05:03:22Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.02 psi 2024-11-06T05:03:22Z- handleDepth()- Depth_Keller msg: 1730869401.15 epoch sec; seqNo:4167; depth 51.13 2024-11-06T05:03:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869401.16 epoch sec; seqNo:4167; 4061501.41 north; 594792.70 east; 36.69 lat; -121.94 lon; 51.13 depth; utmZone: 10 2024-11-06T05:03:22Z- AHRS_M2 msg: 1730869401.55 epoch sec; seqNo:3909 2024-11-06T05:03:22Z- AHRS_M2 msg: 0.14 phi; 0.00 theta; 6.02 psi 2024-11-06T05:03:22Z- handleDepth()- Depth_Keller msg: 1730869401.56 epoch sec; seqNo:4168; depth 51.13 2024-11-06T05:03:22Z- handleDVL() - DVL msg epochMillisec: 1730869401.57 2024-11-06T05:03:22Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:22Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:22Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:22Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:03:22Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:03:22Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:03:22Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:03:22Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:03:22Z- handleDvl() - RDI_Pathfinder msg: 1730869401.57 sec; seqNo:1450; #beams:0 2024-11-06T05:03:22Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:03:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.08 , -0.11 2024-11-06T05:03:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869401.58 epoch sec; seqNo:4168; 4061501.79 north; 594792.70 east; 36.69 lat; -121.94 lon; 51.13 depth; utmZone: 10 2024-11-06T05:03:22Z- AHRS_M2 msg: 1730869401.95 epoch sec; seqNo:3910 2024-11-06T05:03:22Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.02 psi 2024-11-06T05:03:22Z- handleDepth()- Depth_Keller msg: 1730869401.96 epoch sec; seqNo:4169; depth 51.13 2024-11-06T05:03:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869401.97 epoch sec; seqNo:4169; 4061502.17 north; 594792.69 east; 36.69 lat; -121.94 lon; 51.13 depth; utmZone: 10 2024-11-06T05:03:22Z- AHRS_M2 msg: 1730869402.36 epoch sec; seqNo:3911 2024-11-06T05:03:22Z- AHRS_M2 msg: 0.13 phi; 0.01 theta; 6.02 psi 2024-11-06T05:03:22Z- handleDepth()- Depth_Keller msg: 1730869402.37 epoch sec; seqNo:4170; depth 51.12 2024-11-06T05:03:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869402.38 epoch sec; seqNo:4170; 4061502.55 north; 594792.69 east; 36.69 lat; -121.94 lon; 51.12 depth; utmZone: 10 2024-11-06T05:03:22Z- 13 msgs in 200 ms and 14 handle calls 2024-11-06T05:03:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869400.80 2024-11-06T05:03:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:22Z- waiting for 1730869404.80, time is 1730869402.38 2024-11-06T05:03:22Z- handleDepth()- Depth_Keller msg: 1730869402.78 epoch sec; seqNo:4171; depth 51.12 2024-11-06T05:03:22Z- handleDVL() - DVL msg epochMillisec: 1730869402.81 2024-11-06T05:03:22Z- handleDvl() - RDI_Pathfinder msg: 1730869402.81 sec; seqNo:1451; #beams:4 2024-11-06T05:03:22Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.12 , 32.62 , 32.45 , 38.23 2024-11-06T05:03:22Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.17 , -0.05 2024-11-06T05:03:22Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869402.83 epoch sec; seqNo:4171; 4061502.91 north; 594791.46 east; 36.69 lat; -121.94 lon; 51.12 depth; utmZone: 10 2024-11-06T05:03:22Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:22Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869400.80 2024-11-06T05:03:22Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:22Z- waiting for 1730869404.80, time is 1730869402.83 2024-11-06T05:03:23Z- AHRS_M2 msg: 1730869403.17 epoch sec; seqNo:3912 2024-11-06T05:03:23Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.01 psi 2024-11-06T05:03:23Z- handleDepth()- Depth_Keller msg: 1730869403.17 epoch sec; seqNo:4172; depth 51.12 2024-11-06T05:03:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869403.19 epoch sec; seqNo:4172; 4061503.29 north; 594791.46 east; 36.69 lat; -121.94 lon; 51.12 depth; utmZone: 10 2024-11-06T05:03:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869400.80 2024-11-06T05:03:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:23Z- waiting for 1730869404.80, time is 1730869403.19 2024-11-06T05:03:23Z- AHRS_M2 msg: 1730869403.57 epoch sec; seqNo:3913 2024-11-06T05:03:23Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.01 psi 2024-11-06T05:03:23Z- handleDepth()- Depth_Keller msg: 1730869403.58 epoch sec; seqNo:4173; depth 51.12 2024-11-06T05:03:23Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869403.59 epoch sec; seqNo:4173; 4061503.29 north; 594791.46 east; 36.69 lat; -121.94 lon; 51.12 depth; utmZone: 10 2024-11-06T05:03:23Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:23Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869400.80 2024-11-06T05:03:23Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:23Z- waiting for 1730869404.80, time is 1730869403.59 2024-11-06T05:03:23Z- AHRS_M2 msg: 1730869403.97 epoch sec; seqNo:3914 2024-11-06T05:03:23Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.00 psi 2024-11-06T05:03:23Z- handleDepth()- Depth_Keller msg: 1730869403.97 epoch sec; seqNo:4174; depth 51.12 2024-11-06T05:03:23Z- handleDVL() - DVL msg epochMillisec: 1730869403.98 2024-11-06T05:03:23Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:23Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:23Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:23Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:03:23Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:03:23Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:03:23Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:03:23Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:03:23Z- handleDvl() - RDI_Pathfinder msg: 1730869403.98 sec; seqNo:1452; #beams:0 2024-11-06T05:03:23Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:03:23Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.17 , -0.05 2024-11-06T05:03:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869404.00 epoch sec; seqNo:4174; 4061503.67 north; 594791.46 east; 36.69 lat; -121.94 lon; 51.12 depth; utmZone: 10 2024-11-06T05:03:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869400.80 2024-11-06T05:03:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:24Z- waiting for 1730869404.80, time is 1730869404.00 2024-11-06T05:03:24Z- AHRS_M2 msg: 1730869404.38 epoch sec; seqNo:3915 2024-11-06T05:03:24Z- AHRS_M2 msg: 0.13 phi; 0.02 theta; 6.00 psi 2024-11-06T05:03:24Z- handleDepth()- Depth_Keller msg: 1730869404.39 epoch sec; seqNo:4175; depth 51.12 2024-11-06T05:03:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869404.40 epoch sec; seqNo:4175; 4061504.05 north; 594791.45 east; 36.69 lat; -121.94 lon; 51.12 depth; utmZone: 10 2024-11-06T05:03:24Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869400.80 2024-11-06T05:03:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:24Z- waiting for 1730869404.80, time is 1730869404.40 2024-11-06T05:03:24Z- AHRS_M2 msg: 1730869404.78 epoch sec; seqNo:3916 2024-11-06T05:03:24Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 6.00 psi 2024-11-06T05:03:24Z- handleDepth()- Depth_Keller msg: 1730869404.79 epoch sec; seqNo:4176; depth 51.14 2024-11-06T05:03:24Z- handleDVL() - DVL msg epochMillisec: 1730869404.82 2024-11-06T05:03:24Z- handleDvl() - RDI_Pathfinder msg: 1730869404.82 sec; seqNo:1453; #beams:4 2024-11-06T05:03:24Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.29 , 32.11 , 32.28 , 37.89 2024-11-06T05:03:24Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.15 , 0.03 2024-11-06T05:03:24Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869404.84 epoch sec; seqNo:4176; 4061504.43 north; 594791.45 east; 36.69 lat; -121.94 lon; 51.14 depth; utmZone: 10 2024-11-06T05:03:24Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:24Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869400.80 2024-11-06T05:03:24Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:24Z- waiting for 1730869404.80, time is 1730869404.84 2024-11-06T05:03:24Z- LcmTrn::updateTrn() >>>> pose time 1730869404.84, meas time 1730869404.82 <<<< 2024-11-06T05:03:24Z- LcmTrn::updateTrn() equating pose 1730869404.82 and meas times 1730869404.82 2024-11-06T05:03:24Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869404.82 2024-11-06T05:03:24Z- TerrainNav::motionUpdate() - currEstimate = (4061504, 594791.45, 51.14) 2024-11-06T05:03:24Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:03:24Z- TerrainNav:: SubcloudNIS is 0.17, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:03:24Z- TNavPF::Projecting Measurements 2024-11-06T05:03:24Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:03:24Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:03:24Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:03:24Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:03:24Z- TNavFilter::4 good beams. 2024-11-06T05:03:24Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:03:24Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:03:24Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:03:24Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:03:24Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:03:24Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:03:24Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:03:24Z- TerrainMapOctree: (vehN, vehE ) = (4061504.43, 594791.45). 2024-11-06T05:03:24Z- TerrainMapOctree: Min Distance = 249.04. 2024-11-06T05:03:24Z- TerrainMapOctree: Using tile 5. 2024-11-06T05:03:25Z- Forcing Subcloud Comparison 2024-11-06T05:03:25Z- Weighting particles with subcloud comparison 2024-11-06T05:03:25Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:25Z- meanExpectedMeasDiff: 2.619821 delta_rms_squared: 1.036184 alpha: 0.981064 2024-11-06T05:03:25Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:25Z- meanExpectedMeasDiff: -2.303135 delta_rms_squared: 0.770748 alpha: 0.974707 2024-11-06T05:03:25Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:25Z- meanExpectedMeasDiff: -1.361898 delta_rms_squared: 0.934747 alpha: 0.979052 2024-11-06T05:03:25Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:25Z- meanExpectedMeasDiff: 1.232819 delta_rms_squared: 1.243347 alpha: 0.984169 2024-11-06T05:03:25Z- Subcloud NIS: 0.195232 2024-11-06T05:03:25Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:03:25Z- TNavPF:: sumWeights = 0.096658 2024-11-06T05:03:25Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:03:25Z- TNavPF::First Term of Map Covariance Matrix is 0.60 2024-11-06T05:03:25Z- TNavPF::Calculated NIS Value : 0.04 numBeams normalized NIS: 0.01 2024-11-06T05:03:25Z- TerrainNav::Resampling particle filter... 2024-11-06T05:03:25Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869404.82 sec, ping # 0. 2024-11-06T05:03:25Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:03:25Z- variances([0], [2], and psi_berg) = 0.896767 0.541621 0.000000 2024-11-06T05:03:25Z- publishEstimatesDecoder() - at 1730869404.82 sec 2024-11-06T05:03:25Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:03:25Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061450.65 594795.26 58.96 2024-11-06T05:03:25Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061447.74 594796.06 59.93 lat/lon: 36.69 -121.94 2024-11-06T05:03:25Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.90 0.54 0.36 0.00 2024-11-06T05:03:25Z- AHRS_M2 msg: 1730869405.18 epoch sec; seqNo:3917 2024-11-06T05:03:25Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 6.00 psi 2024-11-06T05:03:25Z- handleDepth()- Depth_Keller msg: 1730869405.19 epoch sec; seqNo:4177; depth 51.15 2024-11-06T05:03:25Z- handleDVL() - DVL msg epochMillisec: 1730869405.20 2024-11-06T05:03:25Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:25Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:25Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:25Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:03:25Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:03:25Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:03:25Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:03:25Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:03:25Z- handleDvl() - RDI_Pathfinder msg: 1730869405.20 sec; seqNo:1454; #beams:0 2024-11-06T05:03:25Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:03:25Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.15 , 0.03 2024-11-06T05:03:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869405.23 epoch sec; seqNo:4177; 4061504.81 north; 594791.44 east; 36.69 lat; -121.94 lon; 51.15 depth; utmZone: 10 2024-11-06T05:03:25Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869404.82 2024-11-06T05:03:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:25Z- waiting for 1730869408.82, time is 1730869405.23 2024-11-06T05:03:25Z- AHRS_M2 msg: 1730869405.59 epoch sec; seqNo:3918 2024-11-06T05:03:25Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 6.00 psi 2024-11-06T05:03:25Z- handleDepth()- Depth_Keller msg: 1730869405.59 epoch sec; seqNo:4178; depth 51.14 2024-11-06T05:03:25Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869405.60 epoch sec; seqNo:4178; 4061505.17 north; 594790.22 east; 36.69 lat; -121.94 lon; 51.14 depth; utmZone: 10 2024-11-06T05:03:25Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:25Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869404.82 2024-11-06T05:03:25Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:25Z- waiting for 1730869408.82, time is 1730869405.60 2024-11-06T05:03:25Z- AHRS_M2 msg: 1730869405.99 epoch sec; seqNo:3919 2024-11-06T05:03:25Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 6.00 psi 2024-11-06T05:03:26Z- handleDepth()- Depth_Keller msg: 1730869406.00 epoch sec; seqNo:4179; depth 51.15 2024-11-06T05:03:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869406.01 epoch sec; seqNo:4179; 4061505.55 north; 594790.21 east; 36.69 lat; -121.94 lon; 51.15 depth; utmZone: 10 2024-11-06T05:03:26Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869404.82 2024-11-06T05:03:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:26Z- waiting for 1730869408.82, time is 1730869406.01 2024-11-06T05:03:26Z- handleDepth()- Depth_Keller msg: 1730869406.42 epoch sec; seqNo:4180; depth 51.14 2024-11-06T05:03:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869406.43 epoch sec; seqNo:4180; 4061505.93 north; 594790.21 east; 36.69 lat; -121.94 lon; 51.14 depth; utmZone: 10 2024-11-06T05:03:26Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:03:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869404.82 2024-11-06T05:03:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:26Z- waiting for 1730869408.82, time is 1730869406.43 2024-11-06T05:03:26Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T05:03:26Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T05:03:26Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T05:03:26Z- AHRS_M2 msg: 1730869406.79 epoch sec; seqNo:3920 2024-11-06T05:03:26Z- AHRS_M2 msg: 0.13 phi; 0.03 theta; 6.00 psi 2024-11-06T05:03:26Z- handleDepth()- Depth_Keller msg: 1730869406.82 epoch sec; seqNo:4181; depth 51.16 2024-11-06T05:03:26Z- handleDVL() - DVL msg epochMillisec: 1730869406.85 2024-11-06T05:03:26Z- handleDvl() - RDI_Pathfinder msg: 1730869406.85 sec; seqNo:1455; #beams:4 2024-11-06T05:03:26Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 31.09 , 32.79 , 37.89 2024-11-06T05:03:26Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , 0.05 2024-11-06T05:03:26Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869406.87 epoch sec; seqNo:4181; 4061506.31 north; 594790.21 east; 36.69 lat; -121.94 lon; 51.16 depth; utmZone: 10 2024-11-06T05:03:26Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:26Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869404.82 2024-11-06T05:03:26Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:26Z- waiting for 1730869408.82, time is 1730869406.87 2024-11-06T05:03:27Z- AHRS_M2 msg: 1730869407.20 epoch sec; seqNo:3921 2024-11-06T05:03:27Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 5.99 psi 2024-11-06T05:03:27Z- handleDepth()- Depth_Keller msg: 1730869407.21 epoch sec; seqNo:4182; depth 51.17 2024-11-06T05:03:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869407.22 epoch sec; seqNo:4182; 4061506.69 north; 594790.20 east; 36.69 lat; -121.94 lon; 51.17 depth; utmZone: 10 2024-11-06T05:03:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869404.82 2024-11-06T05:03:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:27Z- waiting for 1730869408.82, time is 1730869407.22 2024-11-06T05:03:27Z- AHRS_M2 msg: 1730869407.61 epoch sec; seqNo:3922 2024-11-06T05:03:27Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 6.00 psi 2024-11-06T05:03:27Z- handleDepth()- Depth_Keller msg: 1730869407.62 epoch sec; seqNo:4183; depth 51.16 2024-11-06T05:03:27Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869407.63 epoch sec; seqNo:4183; 4061507.07 north; 594790.20 east; 36.69 lat; -121.94 lon; 51.16 depth; utmZone: 10 2024-11-06T05:03:27Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:27Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869404.82 2024-11-06T05:03:27Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:27Z- waiting for 1730869408.82, time is 1730869407.63 2024-11-06T05:03:28Z- handleDepth()- Depth_Keller msg: 1730869408.03 epoch sec; seqNo:4184; depth 51.16 2024-11-06T05:03:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869408.04 epoch sec; seqNo:4184; 4061507.45 north; 594790.19 east; 36.69 lat; -121.94 lon; 51.16 depth; utmZone: 10 2024-11-06T05:03:28Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:03:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869404.82 2024-11-06T05:03:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:28Z- waiting for 1730869408.82, time is 1730869408.04 2024-11-06T05:03:28Z- AHRS_M2 msg: 1730869408.41 epoch sec; seqNo:3923 2024-11-06T05:03:28Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 6.00 psi 2024-11-06T05:03:28Z- handleDepth()- Depth_Keller msg: 1730869408.42 epoch sec; seqNo:4185; depth 51.16 2024-11-06T05:03:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869408.43 epoch sec; seqNo:4185; 4061507.82 north; 594790.19 east; 36.69 lat; -121.94 lon; 51.16 depth; utmZone: 10 2024-11-06T05:03:28Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869404.82 2024-11-06T05:03:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:28Z- waiting for 1730869408.82, time is 1730869408.43 2024-11-06T05:03:28Z- AHRS_M2 msg: 1730869408.82 epoch sec; seqNo:3924 2024-11-06T05:03:28Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 6.00 psi 2024-11-06T05:03:28Z- handleDepth()- Depth_Keller msg: 1730869408.83 epoch sec; seqNo:4186; depth 51.15 2024-11-06T05:03:28Z- handleDVL() - DVL msg epochMillisec: 1730869408.86 2024-11-06T05:03:28Z- handleDvl() - RDI_Pathfinder msg: 1730869408.86 sec; seqNo:1456; #beams:4 2024-11-06T05:03:28Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 30.24 , 32.79 , 37.72 2024-11-06T05:03:28Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.00 , 0.03 2024-11-06T05:03:28Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869408.88 epoch sec; seqNo:4186; 4061508.20 north; 594790.18 east; 36.69 lat; -121.94 lon; 51.15 depth; utmZone: 10 2024-11-06T05:03:28Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:28Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869404.82 2024-11-06T05:03:28Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:28Z- waiting for 1730869408.82, time is 1730869408.88 2024-11-06T05:03:28Z- LcmTrn::updateTrn() >>>> pose time 1730869408.88, meas time 1730869408.86 <<<< 2024-11-06T05:03:28Z- LcmTrn::updateTrn() equating pose 1730869408.86 and meas times 1730869408.86 2024-11-06T05:03:28Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869408.86 2024-11-06T05:03:28Z- TerrainNav::motionUpdate() - currEstimate = (4061508, 594790.18, 51.15) 2024-11-06T05:03:28Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:03:28Z- TerrainNav:: SubcloudNIS is 0.20, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:03:29Z- TNavPF::Projecting Measurements 2024-11-06T05:03:29Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:03:29Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:03:29Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:03:29Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:03:29Z- TNavFilter::4 good beams. 2024-11-06T05:03:29Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:03:29Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:03:29Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:03:29Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:03:29Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:03:29Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:03:29Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:03:29Z- TerrainMapOctree: (vehN, vehE ) = (4061508.20, 594790.18). 2024-11-06T05:03:29Z- TerrainMapOctree: Min Distance = 245.11. 2024-11-06T05:03:29Z- TerrainMapOctree: Using tile 5. 2024-11-06T05:03:29Z- Forcing Subcloud Comparison 2024-11-06T05:03:29Z- Weighting particles with subcloud comparison 2024-11-06T05:03:29Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:29Z- meanExpectedMeasDiff: 2.626231 delta_rms_squared: 0.952850 alpha: 0.979442 2024-11-06T05:03:29Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:29Z- meanExpectedMeasDiff: -4.034288 delta_rms_squared: 0.687616 alpha: 0.971736 2024-11-06T05:03:29Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:29Z- meanExpectedMeasDiff: -0.368290 delta_rms_squared: 0.823482 alpha: 0.976289 2024-11-06T05:03:29Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:29Z- meanExpectedMeasDiff: 1.570549 delta_rms_squared: 0.765859 alpha: 0.974550 2024-11-06T05:03:29Z- Subcloud NIS: 0.226829 2024-11-06T05:03:29Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:03:29Z- TNavPF:: sumWeights = 0.021567 2024-11-06T05:03:29Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:03:29Z- TNavPF::First Term of Map Covariance Matrix is 0.17 2024-11-06T05:03:29Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T05:03:29Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869408.86 sec, ping # 0. 2024-11-06T05:03:29Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:03:29Z- variances([0], [2], and psi_berg) = 0.880780 0.539431 0.000000 2024-11-06T05:03:29Z- publishEstimatesDecoder() - at 1730869408.86 sec 2024-11-06T05:03:29Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:03:29Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061452.67 594795.74 60.14 2024-11-06T05:03:29Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061451.50 594794.79 60.04 lat/lon: 36.69 -121.94 2024-11-06T05:03:29Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.88 0.54 0.32 0.00 2024-11-06T05:03:29Z- AHRS_M2 msg: 1730869409.22 epoch sec; seqNo:3925 2024-11-06T05:03:29Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 6.00 psi 2024-11-06T05:03:29Z- handleDepth()- Depth_Keller msg: 1730869409.23 epoch sec; seqNo:4187; depth 51.13 2024-11-06T05:03:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869409.26 epoch sec; seqNo:4187; 4061508.20 north; 594790.18 east; 36.69 lat; -121.94 lon; 51.13 depth; utmZone: 10 2024-11-06T05:03:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869408.86 2024-11-06T05:03:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:29Z- waiting for 1730869412.86, time is 1730869409.26 2024-11-06T05:03:29Z- AHRS_M2 msg: 1730869409.63 epoch sec; seqNo:3926 2024-11-06T05:03:29Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 6.00 psi 2024-11-06T05:03:29Z- handleDepth()- Depth_Keller msg: 1730869409.64 epoch sec; seqNo:4188; depth 51.13 2024-11-06T05:03:29Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869409.66 epoch sec; seqNo:4188; 4061508.57 north; 594788.96 east; 36.69 lat; -121.94 lon; 51.13 depth; utmZone: 10 2024-11-06T05:03:29Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:29Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869408.86 2024-11-06T05:03:29Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:29Z- waiting for 1730869412.86, time is 1730869409.66 2024-11-06T05:03:30Z- AHRS_M2 msg: 1730869410.04 epoch sec; seqNo:3927 2024-11-06T05:03:30Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 6.00 psi 2024-11-06T05:03:30Z- handleDepth()- Depth_Keller msg: 1730869410.04 epoch sec; seqNo:4189; depth 51.11 2024-11-06T05:03:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869410.05 epoch sec; seqNo:4189; 4061508.95 north; 594788.96 east; 36.69 lat; -121.94 lon; 51.11 depth; utmZone: 10 2024-11-06T05:03:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869408.86 2024-11-06T05:03:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:30Z- waiting for 1730869412.86, time is 1730869410.05 2024-11-06T05:03:30Z- AHRS_M2 msg: 1730869410.44 epoch sec; seqNo:3928 2024-11-06T05:03:30Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 6.01 psi 2024-11-06T05:03:30Z- handleDepth()- Depth_Keller msg: 1730869410.44 epoch sec; seqNo:4190; depth 51.09 2024-11-06T05:03:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869410.45 epoch sec; seqNo:4190; 4061509.33 north; 594788.95 east; 36.69 lat; -121.94 lon; 51.09 depth; utmZone: 10 2024-11-06T05:03:30Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:30Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869408.86 2024-11-06T05:03:30Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:30Z- waiting for 1730869412.86, time is 1730869410.45 2024-11-06T05:03:30Z- AHRS_M2 msg: 1730869410.85 epoch sec; seqNo:3929 2024-11-06T05:03:30Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 6.01 psi 2024-11-06T05:03:30Z- handleDepth()- Depth_Keller msg: 1730869410.86 epoch sec; seqNo:4191; depth 51.07 2024-11-06T05:03:30Z- handleDVL() - DVL msg epochMillisec: 1730869410.88 2024-11-06T05:03:30Z- handleDvl() - RDI_Pathfinder msg: 1730869410.88 sec; seqNo:1457; #beams:4 2024-11-06T05:03:30Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.46 , 29.90 , 32.62 , 37.55 2024-11-06T05:03:30Z- handleDvl() - RDI_Pathfinder msg: velocities 0.87 , 0.02 , -0.04 2024-11-06T05:03:30Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869410.91 epoch sec; seqNo:4191; 4061509.71 north; 594788.95 east; 36.69 lat; -121.94 lon; 51.07 depth; utmZone: 10 2024-11-06T05:03:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869408.86 2024-11-06T05:03:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:31Z- waiting for 1730869412.86, time is 1730869410.91 2024-11-06T05:03:31Z- AHRS_M2 msg: 1730869411.24 epoch sec; seqNo:3930 2024-11-06T05:03:31Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 6.01 psi 2024-11-06T05:03:31Z- handleDepth()- Depth_Keller msg: 1730869411.25 epoch sec; seqNo:4192; depth 51.05 2024-11-06T05:03:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869411.27 epoch sec; seqNo:4192; 4061510.08 north; 594788.94 east; 36.69 lat; -121.94 lon; 51.05 depth; utmZone: 10 2024-11-06T05:03:31Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869408.86 2024-11-06T05:03:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:31Z- waiting for 1730869412.86, time is 1730869411.27 2024-11-06T05:03:31Z- AHRS_M2 msg: 1730869411.65 epoch sec; seqNo:3931 2024-11-06T05:03:31Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 6.01 psi 2024-11-06T05:03:31Z- handleDepth()- Depth_Keller msg: 1730869411.65 epoch sec; seqNo:4193; depth 51.04 2024-11-06T05:03:31Z- handleDVL() - DVL msg epochMillisec: 1730869411.67 2024-11-06T05:03:31Z- handleDvl() - RDI_Pathfinder msg: 1730869411.67 sec; seqNo:1458; #beams:4 2024-11-06T05:03:31Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.63 , 29.73 , 32.28 , 37.38 2024-11-06T05:03:31Z- handleDvl() - RDI_Pathfinder msg: velocities 0.89 , -0.00 , -0.06 2024-11-06T05:03:31Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869411.70 epoch sec; seqNo:4193; 4061510.46 north; 594788.94 east; 36.69 lat; -121.94 lon; 51.04 depth; utmZone: 10 2024-11-06T05:03:31Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:31Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869408.86 2024-11-06T05:03:31Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:31Z- waiting for 1730869412.86, time is 1730869411.70 2024-11-06T05:03:32Z- AHRS_M2 msg: 1730869412.05 epoch sec; seqNo:3932 2024-11-06T05:03:32Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 6.01 psi 2024-11-06T05:03:32Z- handleDepth()- Depth_Keller msg: 1730869412.06 epoch sec; seqNo:4194; depth 51.01 2024-11-06T05:03:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869412.07 epoch sec; seqNo:4194; 4061510.84 north; 594788.93 east; 36.69 lat; -121.94 lon; 51.01 depth; utmZone: 10 2024-11-06T05:03:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869408.86 2024-11-06T05:03:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:32Z- waiting for 1730869412.86, time is 1730869412.07 2024-11-06T05:03:32Z- AHRS_M2 msg: 1730869412.45 epoch sec; seqNo:3933 2024-11-06T05:03:32Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 6.01 psi 2024-11-06T05:03:32Z- handleDepth()- Depth_Keller msg: 1730869412.46 epoch sec; seqNo:4195; depth 50.98 2024-11-06T05:03:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869412.48 epoch sec; seqNo:4195; 4061511.22 north; 594788.93 east; 36.69 lat; -121.94 lon; 50.98 depth; utmZone: 10 2024-11-06T05:03:32Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:32Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869408.86 2024-11-06T05:03:32Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:32Z- waiting for 1730869412.86, time is 1730869412.48 2024-11-06T05:03:32Z- AHRS_M2 msg: 1730869412.86 epoch sec; seqNo:3934 2024-11-06T05:03:32Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 6.01 psi 2024-11-06T05:03:32Z- handleDepth()- Depth_Keller msg: 1730869412.87 epoch sec; seqNo:4196; depth 50.95 2024-11-06T05:03:32Z- handleDVL() - DVL msg epochMillisec: 1730869412.89 2024-11-06T05:03:32Z- handleDvl() - RDI_Pathfinder msg: 1730869412.89 sec; seqNo:1459; #beams:4 2024-11-06T05:03:32Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.29 , 29.73 , 32.28 , 37.21 2024-11-06T05:03:32Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.02 , -0.08 2024-11-06T05:03:32Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869412.91 epoch sec; seqNo:4196; 4061511.60 north; 594788.93 east; 36.69 lat; -121.94 lon; 50.95 depth; utmZone: 10 2024-11-06T05:03:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869408.86 2024-11-06T05:03:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:33Z- waiting for 1730869412.86, time is 1730869412.91 2024-11-06T05:03:33Z- LcmTrn::updateTrn() >>>> pose time 1730869412.91, meas time 1730869412.89 <<<< 2024-11-06T05:03:33Z- LcmTrn::updateTrn() equating pose 1730869412.89 and meas times 1730869412.89 2024-11-06T05:03:33Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869412.89 2024-11-06T05:03:33Z- TerrainNav::motionUpdate() - currEstimate = (4061512, 594788.93, 50.95) 2024-11-06T05:03:33Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:03:33Z- TerrainNav:: SubcloudNIS is 0.23, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:03:33Z- TNavPF::Projecting Measurements 2024-11-06T05:03:33Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:03:33Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:03:33Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:03:33Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:03:33Z- TNavFilter::4 good beams. 2024-11-06T05:03:33Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:03:33Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:03:33Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:03:33Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:03:33Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:03:33Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:03:33Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:03:33Z- TerrainMapOctree: (vehN, vehE ) = (4061511.60, 594788.93). 2024-11-06T05:03:33Z- TerrainMapOctree: Min Distance = 241.56. 2024-11-06T05:03:33Z- TerrainMapOctree: Using tile 5. 2024-11-06T05:03:33Z- Forcing Subcloud Comparison 2024-11-06T05:03:33Z- Weighting particles with subcloud comparison 2024-11-06T05:03:33Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:33Z- meanExpectedMeasDiff: 2.091308 delta_rms_squared: 0.906817 alpha: 0.978421 2024-11-06T05:03:33Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:33Z- meanExpectedMeasDiff: -4.664223 delta_rms_squared: 0.652162 alpha: 0.970245 2024-11-06T05:03:33Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:33Z- meanExpectedMeasDiff: -0.409647 delta_rms_squared: 0.703468 alpha: 0.972355 2024-11-06T05:03:33Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:33Z- meanExpectedMeasDiff: 1.229510 delta_rms_squared: 0.714406 alpha: 0.972767 2024-11-06T05:03:33Z- Subcloud NIS: 0.259051 2024-11-06T05:03:33Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:03:33Z- TNavPF:: sumWeights = 0.014247 2024-11-06T05:03:33Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:03:33Z- TNavPF::First Term of Map Covariance Matrix is 0.09 2024-11-06T05:03:33Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T05:03:33Z- TerrainNav::Resampling particle filter... 2024-11-06T05:03:33Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869412.89 sec, ping # 0. 2024-11-06T05:03:33Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:03:33Z- variances([0], [2], and psi_berg) = 0.841722 0.535696 0.000000 2024-11-06T05:03:33Z- publishEstimatesDecoder() - at 1730869412.89 sec 2024-11-06T05:03:33Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:03:33Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061457.92 594792.67 59.43 2024-11-06T05:03:33Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061454.89 594793.53 60.08 lat/lon: 36.69 -121.94 2024-11-06T05:03:33Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.84 0.54 0.33 0.00 2024-11-06T05:03:33Z- AHRS_M2 msg: 1730869413.26 epoch sec; seqNo:3935 2024-11-06T05:03:33Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 6.01 psi 2024-11-06T05:03:33Z- handleDepth()- Depth_Keller msg: 1730869413.27 epoch sec; seqNo:4197; depth 50.92 2024-11-06T05:03:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869413.30 epoch sec; seqNo:4197; 4061511.60 north; 594788.93 east; 36.69 lat; -121.94 lon; 50.92 depth; utmZone: 10 2024-11-06T05:03:33Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869412.89 2024-11-06T05:03:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:33Z- waiting for 1730869416.89, time is 1730869413.30 2024-11-06T05:03:33Z- AHRS_M2 msg: 1730869413.68 epoch sec; seqNo:3936 2024-11-06T05:03:33Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 6.00 psi 2024-11-06T05:03:33Z- handleDepth()- Depth_Keller msg: 1730869413.68 epoch sec; seqNo:4198; depth 50.89 2024-11-06T05:03:33Z- handleDVL() - DVL msg epochMillisec: 1730869413.71 2024-11-06T05:03:33Z- handleDvl() - RDI_Pathfinder msg: 1730869413.71 sec; seqNo:1460; #beams:4 2024-11-06T05:03:33Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.12 , 29.56 , 32.28 , 37.21 2024-11-06T05:03:33Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.07 , -0.07 2024-11-06T05:03:33Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869413.76 epoch sec; seqNo:4198; 4061512.36 north; 594788.92 east; 36.69 lat; -121.94 lon; 50.89 depth; utmZone: 10 2024-11-06T05:03:33Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:33Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869412.89 2024-11-06T05:03:33Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:33Z- waiting for 1730869416.89, time is 1730869413.76 2024-11-06T05:03:34Z- AHRS_M2 msg: 1730869414.07 epoch sec; seqNo:3937 2024-11-06T05:03:34Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 6.00 psi 2024-11-06T05:03:34Z- handleDepth()- Depth_Keller msg: 1730869414.08 epoch sec; seqNo:4199; depth 50.86 2024-11-06T05:03:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869414.09 epoch sec; seqNo:4199; 4061512.36 north; 594788.92 east; 36.69 lat; -121.94 lon; 50.86 depth; utmZone: 10 2024-11-06T05:03:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869412.89 2024-11-06T05:03:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:34Z- waiting for 1730869416.89, time is 1730869414.09 2024-11-06T05:03:34Z- AHRS_M2 msg: 1730869414.47 epoch sec; seqNo:3938 2024-11-06T05:03:34Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 6.00 psi 2024-11-06T05:03:34Z- handleDepth()- Depth_Keller msg: 1730869414.48 epoch sec; seqNo:4200; depth 50.84 2024-11-06T05:03:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869414.49 epoch sec; seqNo:4200; 4061512.72 north; 594787.69 east; 36.69 lat; -121.94 lon; 50.84 depth; utmZone: 10 2024-11-06T05:03:34Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:34Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869412.89 2024-11-06T05:03:34Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:34Z- waiting for 1730869416.89, time is 1730869414.49 2024-11-06T05:03:34Z- AHRS_M2 msg: 1730869414.88 epoch sec; seqNo:3939 2024-11-06T05:03:34Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 6.00 psi 2024-11-06T05:03:34Z- handleDepth()- Depth_Keller msg: 1730869414.89 epoch sec; seqNo:4201; depth 50.82 2024-11-06T05:03:34Z- handleDVL() - DVL msg epochMillisec: 1730869414.91 2024-11-06T05:03:34Z- handleDvl() - RDI_Pathfinder msg: 1730869414.91 sec; seqNo:1461; #beams:4 2024-11-06T05:03:34Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.95 , 29.73 , 32.45 , 37.21 2024-11-06T05:03:34Z- handleDvl() - RDI_Pathfinder msg: velocities 0.92 , -0.10 , -0.06 2024-11-06T05:03:34Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869414.93 epoch sec; seqNo:4201; 4061513.10 north; 594787.69 east; 36.69 lat; -121.94 lon; 50.82 depth; utmZone: 10 2024-11-06T05:03:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869412.89 2024-11-06T05:03:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:35Z- waiting for 1730869416.89, time is 1730869414.93 2024-11-06T05:03:35Z- AHRS_M2 msg: 1730869415.28 epoch sec; seqNo:3940 2024-11-06T05:03:35Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 6.00 psi 2024-11-06T05:03:35Z- handleDepth()- Depth_Keller msg: 1730869415.29 epoch sec; seqNo:4202; depth 50.79 2024-11-06T05:03:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869415.30 epoch sec; seqNo:4202; 4061513.48 north; 594787.69 east; 36.69 lat; -121.94 lon; 50.79 depth; utmZone: 10 2024-11-06T05:03:35Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869412.89 2024-11-06T05:03:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:35Z- waiting for 1730869416.89, time is 1730869415.30 2024-11-06T05:03:35Z- AHRS_M2 msg: 1730869415.71 epoch sec; seqNo:3941 2024-11-06T05:03:35Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.99 psi 2024-11-06T05:03:35Z- handleDepth()- Depth_Keller msg: 1730869415.71 epoch sec; seqNo:4203; depth 50.76 2024-11-06T05:03:35Z- handleDVL() - DVL msg epochMillisec: 1730869415.74 2024-11-06T05:03:35Z- handleDvl() - RDI_Pathfinder msg: 1730869415.74 sec; seqNo:1462; #beams:4 2024-11-06T05:03:35Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.29 , 29.73 , 32.28 , 37.38 2024-11-06T05:03:35Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.12 , -0.05 2024-11-06T05:03:35Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869415.75 epoch sec; seqNo:4203; 4061513.86 north; 594787.68 east; 36.69 lat; -121.94 lon; 50.76 depth; utmZone: 10 2024-11-06T05:03:35Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:35Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869412.89 2024-11-06T05:03:35Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:35Z- waiting for 1730869416.89, time is 1730869415.75 2024-11-06T05:03:36Z- handleDepth()- Depth_Keller msg: 1730869416.11 epoch sec; seqNo:4204; depth 50.73 2024-11-06T05:03:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869416.12 epoch sec; seqNo:4204; 4061514.24 north; 594787.68 east; 36.69 lat; -121.94 lon; 50.73 depth; utmZone: 10 2024-11-06T05:03:36Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:03:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869412.89 2024-11-06T05:03:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:36Z- waiting for 1730869416.89, time is 1730869416.12 2024-11-06T05:03:36Z- AHRS_M2 msg: 1730869416.50 epoch sec; seqNo:3942 2024-11-06T05:03:36Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.99 psi 2024-11-06T05:03:36Z- handleDepth()- Depth_Keller msg: 1730869416.51 epoch sec; seqNo:4205; depth 50.72 2024-11-06T05:03:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869416.52 epoch sec; seqNo:4205; 4061514.62 north; 594787.67 east; 36.69 lat; -121.94 lon; 50.72 depth; utmZone: 10 2024-11-06T05:03:36Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:36Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869412.89 2024-11-06T05:03:36Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:36Z- waiting for 1730869416.89, time is 1730869416.52 2024-11-06T05:03:36Z- AHRS_M2 msg: 1730869416.90 epoch sec; seqNo:3943 2024-11-06T05:03:36Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.99 psi 2024-11-06T05:03:36Z- handleDepth()- Depth_Keller msg: 1730869416.91 epoch sec; seqNo:4206; depth 50.70 2024-11-06T05:03:36Z- handleDVL() - DVL msg epochMillisec: 1730869416.93 2024-11-06T05:03:36Z- handleDvl() - RDI_Pathfinder msg: 1730869416.93 sec; seqNo:1463; #beams:4 2024-11-06T05:03:36Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.29 , 29.56 , 32.11 , 37.21 2024-11-06T05:03:36Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.13 , -0.04 2024-11-06T05:03:36Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869416.96 epoch sec; seqNo:4206; 4061515.00 north; 594787.67 east; 36.69 lat; -121.94 lon; 50.70 depth; utmZone: 10 2024-11-06T05:03:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869412.89 2024-11-06T05:03:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:37Z- waiting for 1730869416.89, time is 1730869416.96 2024-11-06T05:03:37Z- LcmTrn::updateTrn() >>>> pose time 1730869416.96, meas time 1730869416.93 <<<< 2024-11-06T05:03:37Z- LcmTrn::updateTrn() equating pose 1730869416.93 and meas times 1730869416.93 2024-11-06T05:03:37Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869416.93 2024-11-06T05:03:37Z- TerrainNav::motionUpdate() - currEstimate = (4061515, 594787.67, 50.70) 2024-11-06T05:03:37Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:03:37Z- TerrainNav:: SubcloudNIS is 0.26, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:03:37Z- TNavPF::Projecting Measurements 2024-11-06T05:03:37Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:03:37Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:03:37Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:03:37Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:03:37Z- TNavFilter::4 good beams. 2024-11-06T05:03:37Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:03:37Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:03:37Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:03:37Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:03:37Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:03:37Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:03:37Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:03:37Z- TerrainMapOctree: (vehN, vehE ) = (4061515.00, 594787.67). 2024-11-06T05:03:37Z- TerrainMapOctree: Min Distance = 238.00. 2024-11-06T05:03:37Z- TerrainMapOctree: Using tile 5. 2024-11-06T05:03:37Z- Forcing Subcloud Comparison 2024-11-06T05:03:37Z- Weighting particles with subcloud comparison 2024-11-06T05:03:37Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:37Z- meanExpectedMeasDiff: 2.714335 delta_rms_squared: 1.129486 alpha: 0.982601 2024-11-06T05:03:37Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:37Z- meanExpectedMeasDiff: -4.811715 delta_rms_squared: 0.871127 alpha: 0.977557 2024-11-06T05:03:37Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:37Z- meanExpectedMeasDiff: -0.315771 delta_rms_squared: 0.835370 alpha: 0.976618 2024-11-06T05:03:37Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:37Z- meanExpectedMeasDiff: 1.273679 delta_rms_squared: 0.727747 alpha: 0.973253 2024-11-06T05:03:37Z- Subcloud NIS: 0.300084 2024-11-06T05:03:37Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:03:37Z- TNavPF:: sumWeights = 0.008474 2024-11-06T05:03:37Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:03:37Z- TNavPF::First Term of Map Covariance Matrix is 0.08 2024-11-06T05:03:37Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T05:03:37Z- TerrainNav::Resampling particle filter... 2024-11-06T05:03:37Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869416.93 sec, ping # 0. 2024-11-06T05:03:37Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:03:37Z- variances([0], [2], and psi_berg) = 0.853255 0.684293 0.000000 2024-11-06T05:03:37Z- publishEstimatesDecoder() - at 1730869416.93 sec 2024-11-06T05:03:37Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:03:37Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061461.31 594791.33 59.13 2024-11-06T05:03:37Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061458.14 594792.12 60.09 lat/lon: 36.69 -121.94 2024-11-06T05:03:37Z- LcmTrn::publishEstimatesDecoder() - COVAR: 0.85 0.68 0.32 0.00 2024-11-06T05:03:37Z- AHRS_M2 msg: 1730869417.30 epoch sec; seqNo:3944 2024-11-06T05:03:37Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.99 psi 2024-11-06T05:03:37Z- handleDepth()- Depth_Keller msg: 1730869417.32 epoch sec; seqNo:4207; depth 50.67 2024-11-06T05:03:37Z- handleDVL() - DVL msg epochMillisec: 1730869417.32 2024-11-06T05:03:37Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:37Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:37Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:37Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:03:37Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:03:37Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:03:37Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:03:37Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:03:37Z- handleDvl() - RDI_Pathfinder msg: 1730869417.32 sec; seqNo:1464; #beams:0 2024-11-06T05:03:37Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:03:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.13 , -0.04 2024-11-06T05:03:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869417.34 epoch sec; seqNo:4207; 4061515.37 north; 594787.66 east; 36.69 lat; -121.94 lon; 50.67 depth; utmZone: 10 2024-11-06T05:03:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869416.93 2024-11-06T05:03:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:37Z- waiting for 1730869420.93, time is 1730869417.34 2024-11-06T05:03:37Z- AHRS_M2 msg: 1730869417.74 epoch sec; seqNo:3945 2024-11-06T05:03:37Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.99 psi 2024-11-06T05:03:37Z- handleDepth()- Depth_Keller msg: 1730869417.75 epoch sec; seqNo:4208; depth 50.66 2024-11-06T05:03:37Z- handleDVL() - DVL msg epochMillisec: 1730869417.77 2024-11-06T05:03:37Z- handleDvl() - RDI_Pathfinder msg: 1730869417.77 sec; seqNo:1465; #beams:4 2024-11-06T05:03:37Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.95 , 29.22 , 31.94 , 37.38 2024-11-06T05:03:37Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.12 , -0.02 2024-11-06T05:03:37Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869417.78 epoch sec; seqNo:4208; 4061515.74 north; 594786.44 east; 36.69 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T05:03:37Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:37Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869416.93 2024-11-06T05:03:37Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:37Z- waiting for 1730869420.93, time is 1730869417.78 2024-11-06T05:03:38Z- AHRS_M2 msg: 1730869418.11 epoch sec; seqNo:3946 2024-11-06T05:03:38Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.98 psi 2024-11-06T05:03:38Z- handleDepth()- Depth_Keller msg: 1730869418.12 epoch sec; seqNo:4209; depth 50.66 2024-11-06T05:03:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869418.14 epoch sec; seqNo:4209; 4061515.74 north; 594786.44 east; 36.69 lat; -121.94 lon; 50.66 depth; utmZone: 10 2024-11-06T05:03:38Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869416.93 2024-11-06T05:03:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:38Z- waiting for 1730869420.93, time is 1730869418.14 2024-11-06T05:03:38Z- AHRS_M2 msg: 1730869418.52 epoch sec; seqNo:3947 2024-11-06T05:03:38Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.98 psi 2024-11-06T05:03:38Z- handleDepth()- Depth_Keller msg: 1730869418.53 epoch sec; seqNo:4210; depth 50.65 2024-11-06T05:03:38Z- handleDVL() - DVL msg epochMillisec: 1730869418.53 2024-11-06T05:03:38Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:38Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:38Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:38Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:03:38Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:03:38Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:03:38Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:03:38Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:03:38Z- handleDvl() - RDI_Pathfinder msg: 1730869418.53 sec; seqNo:1466; #beams:0 2024-11-06T05:03:38Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:03:38Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.12 , -0.02 2024-11-06T05:03:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869418.54 epoch sec; seqNo:4210; 4061516.12 north; 594786.44 east; 36.69 lat; -121.94 lon; 50.65 depth; utmZone: 10 2024-11-06T05:03:38Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:38Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869416.93 2024-11-06T05:03:38Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:38Z- waiting for 1730869420.93, time is 1730869418.54 2024-11-06T05:03:38Z- handleAhrs() - platform_orientation item not found in ahrs msg 2024-11-06T05:03:38Z- handleAhrs() - platform_pitch_angle item not found in ahrs msg 2024-11-06T05:03:38Z- handleAhrs() - platform_roll_angle item not found in ahrs msg 2024-11-06T05:03:38Z- AHRS_M2 msg: 1730869418.91 epoch sec; seqNo:3948 2024-11-06T05:03:38Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.98 psi 2024-11-06T05:03:38Z- handleDepth()- Depth_Keller msg: 1730869418.94 epoch sec; seqNo:4211; depth 50.64 2024-11-06T05:03:38Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869418.97 epoch sec; seqNo:4211; 4061516.50 north; 594786.43 east; 36.69 lat; -121.94 lon; 50.64 depth; utmZone: 10 2024-11-06T05:03:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869416.93 2024-11-06T05:03:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:39Z- waiting for 1730869420.93, time is 1730869418.97 2024-11-06T05:03:39Z- AHRS_M2 msg: 1730869419.32 epoch sec; seqNo:3949 2024-11-06T05:03:39Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.98 psi 2024-11-06T05:03:39Z- handleDepth()- Depth_Keller msg: 1730869419.33 epoch sec; seqNo:4212; depth 50.63 2024-11-06T05:03:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869419.34 epoch sec; seqNo:4212; 4061516.88 north; 594786.43 east; 36.69 lat; -121.94 lon; 50.63 depth; utmZone: 10 2024-11-06T05:03:39Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869416.93 2024-11-06T05:03:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:39Z- waiting for 1730869420.93, time is 1730869419.34 2024-11-06T05:03:39Z- AHRS_M2 msg: 1730869419.76 epoch sec; seqNo:3950 2024-11-06T05:03:39Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.98 psi 2024-11-06T05:03:39Z- handleDepth()- Depth_Keller msg: 1730869419.76 epoch sec; seqNo:4213; depth 50.62 2024-11-06T05:03:39Z- handleDVL() - DVL msg epochMillisec: 1730869419.79 2024-11-06T05:03:39Z- handleDvl() - RDI_Pathfinder msg: 1730869419.79 sec; seqNo:1467; #beams:4 2024-11-06T05:03:39Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.95 , 29.05 , 31.60 , 37.38 2024-11-06T05:03:39Z- handleDvl() - RDI_Pathfinder msg: velocities 0.94 , -0.09 , 0.01 2024-11-06T05:03:39Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869419.81 epoch sec; seqNo:4213; 4061517.26 north; 594786.42 east; 36.69 lat; -121.94 lon; 50.62 depth; utmZone: 10 2024-11-06T05:03:39Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:39Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869416.93 2024-11-06T05:03:39Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:39Z- waiting for 1730869420.93, time is 1730869419.81 2024-11-06T05:03:40Z- AHRS_M2 msg: 1730869420.13 epoch sec; seqNo:3951 2024-11-06T05:03:40Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.98 psi 2024-11-06T05:03:40Z- handleDepth()- Depth_Keller msg: 1730869420.14 epoch sec; seqNo:4214; depth 50.60 2024-11-06T05:03:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869420.16 epoch sec; seqNo:4214; 4061517.63 north; 594786.42 east; 36.69 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T05:03:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869416.93 2024-11-06T05:03:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:40Z- waiting for 1730869420.93, time is 1730869420.16 2024-11-06T05:03:40Z- AHRS_M2 msg: 1730869420.54 epoch sec; seqNo:3952 2024-11-06T05:03:40Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.98 psi 2024-11-06T05:03:40Z- handleDepth()- Depth_Keller msg: 1730869420.55 epoch sec; seqNo:4215; depth 50.61 2024-11-06T05:03:40Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869420.56 epoch sec; seqNo:4215; 4061518.01 north; 594786.41 east; 36.69 lat; -121.94 lon; 50.61 depth; utmZone: 10 2024-11-06T05:03:40Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:40Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869416.93 2024-11-06T05:03:40Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:40Z- waiting for 1730869420.93, time is 1730869420.56 2024-11-06T05:03:40Z- AHRS_M2 msg: 1730869420.94 epoch sec; seqNo:3953 2024-11-06T05:03:40Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.98 psi 2024-11-06T05:03:40Z- handleDepth()- Depth_Keller msg: 1730869420.95 epoch sec; seqNo:4216; depth 50.60 2024-11-06T05:03:40Z- handleDVL() - DVL msg epochMillisec: 1730869420.96 2024-11-06T05:03:40Z- handleDvl() - RDI_Pathfinder msg: 1730869420.96 sec; seqNo:1468; #beams:4 2024-11-06T05:03:40Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 41.12 , 28.88 , 31.77 , 37.38 2024-11-06T05:03:40Z- handleDvl() - RDI_Pathfinder msg: velocities 0.93 , -0.06 , 0.03 2024-11-06T05:03:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869421.00 epoch sec; seqNo:4216; 4061518.38 north; 594785.19 east; 36.69 lat; -121.94 lon; 50.60 depth; utmZone: 10 2024-11-06T05:03:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869416.93 2024-11-06T05:03:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:41Z- waiting for 1730869420.93, time is 1730869421.00 2024-11-06T05:03:41Z- LcmTrn::updateTrn() >>>> pose time 1730869421.00, meas time 1730869420.96 <<<< 2024-11-06T05:03:41Z- LcmTrn::updateTrn() equating pose 1730869420.96 and meas times 1730869420.96 2024-11-06T05:03:41Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869420.96 2024-11-06T05:03:41Z- TerrainNav::motionUpdate() - currEstimate = (4061518, 594785.19, 50.60) 2024-11-06T05:03:41Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:03:41Z- TerrainNav:: SubcloudNIS is 0.30, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:03:41Z- TNavPF::Projecting Measurements 2024-11-06T05:03:41Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:03:41Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:03:41Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:03:41Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:03:41Z- TNavFilter::4 good beams. 2024-11-06T05:03:41Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:03:41Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:03:41Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:03:41Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:03:41Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:03:41Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:03:41Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:03:41Z- TerrainMapOctree: (vehN, vehE ) = (4061518.38, 594785.19). 2024-11-06T05:03:41Z- TerrainMapOctree: Min Distance = 234.28. 2024-11-06T05:03:41Z- TerrainMapOctree: Using tile 5. 2024-11-06T05:03:41Z- Forcing Subcloud Comparison 2024-11-06T05:03:41Z- Weighting particles with subcloud comparison 2024-11-06T05:03:41Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:41Z- meanExpectedMeasDiff: 3.618880 delta_rms_squared: 0.900837 alpha: 0.978281 2024-11-06T05:03:41Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:41Z- meanExpectedMeasDiff: -4.109717 delta_rms_squared: 0.975139 alpha: 0.979902 2024-11-06T05:03:41Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:41Z- meanExpectedMeasDiff: 0.126049 delta_rms_squared: 0.906673 alpha: 0.978417 2024-11-06T05:03:41Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:41Z- meanExpectedMeasDiff: 1.808467 delta_rms_squared: 0.994767 alpha: 0.980291 2024-11-06T05:03:41Z- Subcloud NIS: 0.338345 2024-11-06T05:03:41Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:03:41Z- TNavPF:: sumWeights = 0.008103 2024-11-06T05:03:41Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:03:41Z- TNavPF::First Term of Map Covariance Matrix is 0.10 2024-11-06T05:03:41Z- TNavPF::Calculated NIS Value : 0.00 numBeams normalized NIS: 0.00 2024-11-06T05:03:41Z- TerrainNav::Resampling particle filter... 2024-11-06T05:03:41Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869420.96 sec, ping # 0. 2024-11-06T05:03:41Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:03:41Z- variances([0], [2], and psi_berg) = 1.077970 0.756619 0.000000 2024-11-06T05:03:41Z- publishEstimatesDecoder() - at 1730869420.96 sec 2024-11-06T05:03:41Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:03:41Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061464.72 594788.81 58.67 2024-11-06T05:03:41Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061461.66 594789.35 59.92 lat/lon: 36.69 -121.94 2024-11-06T05:03:41Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.08 0.76 0.34 0.00 2024-11-06T05:03:41Z- AHRS_M2 msg: 1730869421.34 epoch sec; seqNo:3954 2024-11-06T05:03:41Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.98 psi 2024-11-06T05:03:41Z- handleDepth()- Depth_Keller msg: 1730869421.35 epoch sec; seqNo:4217; depth 50.59 2024-11-06T05:03:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869421.39 epoch sec; seqNo:4217; 4061518.76 north; 594785.19 east; 36.69 lat; -121.94 lon; 50.59 depth; utmZone: 10 2024-11-06T05:03:41Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869420.96 2024-11-06T05:03:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:41Z- waiting for 1730869424.96, time is 1730869421.39 2024-11-06T05:03:41Z- AHRS_M2 msg: 1730869421.78 epoch sec; seqNo:3955 2024-11-06T05:03:41Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.98 psi 2024-11-06T05:03:41Z- handleDepth()- Depth_Keller msg: 1730869421.79 epoch sec; seqNo:4218; depth 50.57 2024-11-06T05:03:41Z- handleDVL() - DVL msg epochMillisec: 1730869421.82 2024-11-06T05:03:41Z- handleDvl() - RDI_Pathfinder msg: 1730869421.82 sec; seqNo:1469; #beams:4 2024-11-06T05:03:41Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.95 , 28.71 , 31.77 , 37.38 2024-11-06T05:03:41Z- handleDvl() - RDI_Pathfinder msg: velocities 0.91 , -0.03 , 0.04 2024-11-06T05:03:41Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869421.84 epoch sec; seqNo:4218; 4061519.14 north; 594785.18 east; 36.69 lat; -121.94 lon; 50.57 depth; utmZone: 10 2024-11-06T05:03:41Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:41Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869420.96 2024-11-06T05:03:41Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:41Z- waiting for 1730869424.96, time is 1730869421.84 2024-11-06T05:03:42Z- AHRS_M2 msg: 1730869422.15 epoch sec; seqNo:3956 2024-11-06T05:03:42Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.98 psi 2024-11-06T05:03:42Z- handleDepth()- Depth_Keller msg: 1730869422.16 epoch sec; seqNo:4219; depth 50.56 2024-11-06T05:03:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869422.17 epoch sec; seqNo:4219; 4061519.52 north; 594785.18 east; 36.69 lat; -121.94 lon; 50.56 depth; utmZone: 10 2024-11-06T05:03:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869420.96 2024-11-06T05:03:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:42Z- waiting for 1730869424.96, time is 1730869422.17 2024-11-06T05:03:42Z- AHRS_M2 msg: 1730869422.56 epoch sec; seqNo:3957 2024-11-06T05:03:42Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.98 psi 2024-11-06T05:03:42Z- handleDepth()- Depth_Keller msg: 1730869422.56 epoch sec; seqNo:4220; depth 50.54 2024-11-06T05:03:42Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869422.57 epoch sec; seqNo:4220; 4061519.89 north; 594785.17 east; 36.69 lat; -121.94 lon; 50.54 depth; utmZone: 10 2024-11-06T05:03:42Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:42Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869420.96 2024-11-06T05:03:42Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:42Z- waiting for 1730869424.96, time is 1730869422.57 2024-11-06T05:03:42Z- AHRS_M2 msg: 1730869422.96 epoch sec; seqNo:3958 2024-11-06T05:03:42Z- AHRS_M2 msg: 0.13 phi; 0.06 theta; 5.98 psi 2024-11-06T05:03:42Z- handleDepth()- Depth_Keller msg: 1730869422.97 epoch sec; seqNo:4221; depth 50.55 2024-11-06T05:03:42Z- handleDVL() - DVL msg epochMillisec: 1730869422.99 2024-11-06T05:03:42Z- handleDvl() - RDI_Pathfinder msg: 1730869422.99 sec; seqNo:1470; #beams:4 2024-11-06T05:03:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.78 , 28.54 , 31.77 , 37.38 2024-11-06T05:03:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.88 , -0.01 , 0.03 2024-11-06T05:03:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869423.00 epoch sec; seqNo:4221; 4061520.27 north; 594785.17 east; 36.69 lat; -121.94 lon; 50.55 depth; utmZone: 10 2024-11-06T05:03:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869420.96 2024-11-06T05:03:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:43Z- waiting for 1730869424.96, time is 1730869423.00 2024-11-06T05:03:43Z- AHRS_M2 msg: 1730869423.37 epoch sec; seqNo:3959 2024-11-06T05:03:43Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 5.98 psi 2024-11-06T05:03:43Z- handleDepth()- Depth_Keller msg: 1730869423.37 epoch sec; seqNo:4222; depth 50.52 2024-11-06T05:03:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869423.40 epoch sec; seqNo:4222; 4061520.27 north; 594785.17 east; 36.69 lat; -121.94 lon; 50.52 depth; utmZone: 10 2024-11-06T05:03:43Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869420.96 2024-11-06T05:03:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:43Z- waiting for 1730869424.96, time is 1730869423.40 2024-11-06T05:03:43Z- AHRS_M2 msg: 1730869423.82 epoch sec; seqNo:3960 2024-11-06T05:03:43Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 5.98 psi 2024-11-06T05:03:43Z- handleDepth()- Depth_Keller msg: 1730869423.83 epoch sec; seqNo:4223; depth 50.49 2024-11-06T05:03:43Z- handleDVL() - DVL msg epochMillisec: 1730869423.86 2024-11-06T05:03:43Z- handleDvl() - RDI_Pathfinder msg: 1730869423.86 sec; seqNo:1471; #beams:4 2024-11-06T05:03:43Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.95 , 28.54 , 31.94 , 37.55 2024-11-06T05:03:43Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , -0.00 2024-11-06T05:03:43Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869423.87 epoch sec; seqNo:4223; 4061520.65 north; 594785.16 east; 36.69 lat; -121.94 lon; 50.49 depth; utmZone: 10 2024-11-06T05:03:43Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:43Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869420.96 2024-11-06T05:03:43Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:43Z- waiting for 1730869424.96, time is 1730869423.87 2024-11-06T05:03:44Z- AHRS_M2 msg: 1730869424.17 epoch sec; seqNo:3961 2024-11-06T05:03:44Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 5.98 psi 2024-11-06T05:03:44Z- handleDepth()- Depth_Keller msg: 1730869424.18 epoch sec; seqNo:4224; depth 50.47 2024-11-06T05:03:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869424.19 epoch sec; seqNo:4224; 4061521.03 north; 594785.16 east; 36.69 lat; -121.94 lon; 50.47 depth; utmZone: 10 2024-11-06T05:03:44Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869420.96 2024-11-06T05:03:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:44Z- waiting for 1730869424.96, time is 1730869424.19 2024-11-06T05:03:44Z- AHRS_M2 msg: 1730869424.58 epoch sec; seqNo:3962 2024-11-06T05:03:44Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 5.98 psi 2024-11-06T05:03:44Z- handleDepth()- Depth_Keller msg: 1730869424.58 epoch sec; seqNo:4225; depth 50.46 2024-11-06T05:03:44Z- handleDVL() - DVL msg epochMillisec: 1730869424.58 2024-11-06T05:03:44Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:44Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:44Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:44Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:03:44Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:03:44Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:03:44Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:03:44Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:03:44Z- handleDvl() - RDI_Pathfinder msg: 1730869424.58 sec; seqNo:1472; #beams:0 2024-11-06T05:03:44Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:03:44Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , -0.00 2024-11-06T05:03:44Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869424.59 epoch sec; seqNo:4225; 4061521.41 north; 594785.16 east; 36.69 lat; -121.94 lon; 50.46 depth; utmZone: 10 2024-11-06T05:03:44Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:44Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869420.96 2024-11-06T05:03:44Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:44Z- waiting for 1730869424.96, time is 1730869424.59 2024-11-06T05:03:44Z- AHRS_M2 msg: 1730869424.98 epoch sec; seqNo:3963 2024-11-06T05:03:44Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 5.98 psi 2024-11-06T05:03:45Z- handleDepth()- Depth_Keller msg: 1730869425.00 epoch sec; seqNo:4226; depth 50.43 2024-11-06T05:03:45Z- handleDVL() - DVL msg epochMillisec: 1730869425.02 2024-11-06T05:03:45Z- handleDvl() - RDI_Pathfinder msg: 1730869425.02 sec; seqNo:1473; #beams:4 2024-11-06T05:03:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.78 , 28.54 , 31.77 , 37.38 2024-11-06T05:03:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , 0.01 , -0.04 2024-11-06T05:03:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869425.04 epoch sec; seqNo:4226; 4061521.77 north; 594783.93 east; 36.69 lat; -121.94 lon; 50.43 depth; utmZone: 10 2024-11-06T05:03:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869420.96 2024-11-06T05:03:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:45Z- waiting for 1730869424.96, time is 1730869425.04 2024-11-06T05:03:45Z- LcmTrn::updateTrn() >>>> pose time 1730869425.04, meas time 1730869425.02 <<<< 2024-11-06T05:03:45Z- LcmTrn::updateTrn() equating pose 1730869425.02 and meas times 1730869425.02 2024-11-06T05:03:45Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869425.02 2024-11-06T05:03:45Z- TerrainNav::motionUpdate() - currEstimate = (4061522, 594783.93, 50.43) 2024-11-06T05:03:45Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:03:45Z- TerrainNav:: SubcloudNIS is 0.34, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:03:45Z- TNavPF::Projecting Measurements 2024-11-06T05:03:45Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:03:45Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:03:45Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:03:45Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:03:45Z- TNavFilter::4 good beams. 2024-11-06T05:03:45Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:03:45Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:03:45Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:03:45Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:03:45Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:03:45Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:03:45Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:03:45Z- TerrainMapOctree: (vehN, vehE ) = (4061521.77, 594783.93). 2024-11-06T05:03:45Z- TerrainMapOctree: Min Distance = 230.73. 2024-11-06T05:03:45Z- TerrainMapOctree: Using tile 5. 2024-11-06T05:03:45Z- Forcing Subcloud Comparison 2024-11-06T05:03:45Z- Weighting particles with subcloud comparison 2024-11-06T05:03:45Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:45Z- meanExpectedMeasDiff: 2.756902 delta_rms_squared: 0.955656 alpha: 0.979501 2024-11-06T05:03:45Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:45Z- meanExpectedMeasDiff: -3.206902 delta_rms_squared: 1.079807 alpha: 0.981815 2024-11-06T05:03:45Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:45Z- meanExpectedMeasDiff: 0.308199 delta_rms_squared: 0.905901 alpha: 0.978399 2024-11-06T05:03:45Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:45Z- meanExpectedMeasDiff: 2.444898 delta_rms_squared: 0.724407 alpha: 0.973133 2024-11-06T05:03:45Z- Subcloud NIS: 0.374375 2024-11-06T05:03:45Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:03:45Z- TNavPF:: sumWeights = 0.033763 2024-11-06T05:03:45Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:03:45Z- TNavPF::First Term of Map Covariance Matrix is 0.22 2024-11-06T05:03:45Z- TNavPF::Calculated NIS Value : 0.01 numBeams normalized NIS: 0.00 2024-11-06T05:03:45Z- TerrainNav::Resampling particle filter... 2024-11-06T05:03:45Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869425.02 sec, ping # 0. 2024-11-06T05:03:45Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:03:45Z- variances([0], [2], and psi_berg) = 1.337531 0.625466 0.000000 2024-11-06T05:03:45Z- publishEstimatesDecoder() - at 1730869425.02 sec 2024-11-06T05:03:45Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:03:45Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061468.19 594787.55 58.56 2024-11-06T05:03:45Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061465.40 594787.65 59.58 lat/lon: 36.69 -121.94 2024-11-06T05:03:45Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.34 0.63 0.32 0.00 2024-11-06T05:03:45Z- AHRS_M2 msg: 1730869425.41 epoch sec; seqNo:3964 2024-11-06T05:03:45Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 5.98 psi 2024-11-06T05:03:45Z- handleDepth()- Depth_Keller msg: 1730869425.42 epoch sec; seqNo:4227; depth 50.39 2024-11-06T05:03:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869425.46 epoch sec; seqNo:4227; 4061522.15 north; 594783.93 east; 36.69 lat; -121.94 lon; 50.39 depth; utmZone: 10 2024-11-06T05:03:45Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869425.02 2024-11-06T05:03:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:45Z- waiting for 1730869429.02, time is 1730869425.46 2024-11-06T05:03:45Z- AHRS_M2 msg: 1730869425.83 epoch sec; seqNo:3965 2024-11-06T05:03:45Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 5.99 psi 2024-11-06T05:03:45Z- handleDepth()- Depth_Keller msg: 1730869425.85 epoch sec; seqNo:4228; depth 50.36 2024-11-06T05:03:45Z- handleDVL() - DVL msg epochMillisec: 1730869425.87 2024-11-06T05:03:45Z- handleDvl() - RDI_Pathfinder msg: 1730869425.87 sec; seqNo:1474; #beams:4 2024-11-06T05:03:45Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.61 , 28.54 , 31.60 , 37.38 2024-11-06T05:03:45Z- handleDvl() - RDI_Pathfinder msg: velocities 0.86 , -0.01 , -0.07 2024-11-06T05:03:45Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869425.88 epoch sec; seqNo:4228; 4061522.53 north; 594783.92 east; 36.69 lat; -121.94 lon; 50.36 depth; utmZone: 10 2024-11-06T05:03:45Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:45Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869425.02 2024-11-06T05:03:45Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:45Z- waiting for 1730869429.02, time is 1730869425.88 2024-11-06T05:03:46Z- handleDepth()- Depth_Keller msg: 1730869426.22 epoch sec; seqNo:4229; depth 50.34 2024-11-06T05:03:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869426.24 epoch sec; seqNo:4229; 4061522.91 north; 594783.92 east; 36.69 lat; -121.94 lon; 50.34 depth; utmZone: 10 2024-11-06T05:03:46Z- 2 msgs in 200 ms and 3 handle calls 2024-11-06T05:03:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869425.02 2024-11-06T05:03:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:46Z- waiting for 1730869429.02, time is 1730869426.24 2024-11-06T05:03:46Z- AHRS_M2 msg: 1730869426.60 epoch sec; seqNo:3966 2024-11-06T05:03:46Z- AHRS_M2 msg: 0.14 phi; 0.06 theta; 5.98 psi 2024-11-06T05:03:46Z- handleDepth()- Depth_Keller msg: 1730869426.60 epoch sec; seqNo:4230; depth 50.31 2024-11-06T05:03:46Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869426.61 epoch sec; seqNo:4230; 4061522.91 north; 594783.92 east; 36.69 lat; -121.94 lon; 50.31 depth; utmZone: 10 2024-11-06T05:03:46Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:46Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869425.02 2024-11-06T05:03:46Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:46Z- waiting for 1730869429.02, time is 1730869426.61 2024-11-06T05:03:47Z- AHRS_M2 msg: 1730869427.01 epoch sec; seqNo:3967 2024-11-06T05:03:47Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 5.98 psi 2024-11-06T05:03:47Z- handleDepth()- Depth_Keller msg: 1730869427.01 epoch sec; seqNo:4231; depth 50.28 2024-11-06T05:03:47Z- handleDVL() - DVL msg epochMillisec: 1730869427.03 2024-11-06T05:03:47Z- handleDvl() - RDI_Pathfinder msg: 1730869427.03 sec; seqNo:1475; #beams:4 2024-11-06T05:03:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.44 , 28.54 , 31.60 , 37.38 2024-11-06T05:03:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.90 , -0.05 , -0.10 2024-11-06T05:03:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869427.05 epoch sec; seqNo:4231; 4061523.29 north; 594783.92 east; 36.69 lat; -121.94 lon; 50.28 depth; utmZone: 10 2024-11-06T05:03:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869425.02 2024-11-06T05:03:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:47Z- waiting for 1730869429.02, time is 1730869427.05 2024-11-06T05:03:47Z- AHRS_M2 msg: 1730869427.41 epoch sec; seqNo:3968 2024-11-06T05:03:47Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 5.98 psi 2024-11-06T05:03:47Z- handleDepth()- Depth_Keller msg: 1730869427.41 epoch sec; seqNo:4232; depth 50.26 2024-11-06T05:03:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869427.44 epoch sec; seqNo:4232; 4061523.67 north; 594783.91 east; 36.69 lat; -121.94 lon; 50.26 depth; utmZone: 10 2024-11-06T05:03:47Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869425.02 2024-11-06T05:03:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:47Z- waiting for 1730869429.02, time is 1730869427.44 2024-11-06T05:03:47Z- AHRS_M2 msg: 1730869427.82 epoch sec; seqNo:3969 2024-11-06T05:03:47Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 5.98 psi 2024-11-06T05:03:47Z- handleDepth()- Depth_Keller msg: 1730869427.86 epoch sec; seqNo:4233; depth 50.23 2024-11-06T05:03:47Z- handleDVL() - DVL msg epochMillisec: 1730869427.89 2024-11-06T05:03:47Z- handleDvl() - RDI_Pathfinder msg: 1730869427.89 sec; seqNo:1476; #beams:4 2024-11-06T05:03:47Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 40.27 , 28.71 , 31.26 , 37.38 2024-11-06T05:03:47Z- handleDvl() - RDI_Pathfinder msg: velocities 0.95 , -0.09 , -0.10 2024-11-06T05:03:47Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869427.91 epoch sec; seqNo:4233; 4061524.05 north; 594783.91 east; 36.69 lat; -121.94 lon; 50.23 depth; utmZone: 10 2024-11-06T05:03:47Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:47Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869425.02 2024-11-06T05:03:47Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:47Z- waiting for 1730869429.02, time is 1730869427.91 2024-11-06T05:03:48Z- AHRS_M2 msg: 1730869428.22 epoch sec; seqNo:3970 2024-11-06T05:03:48Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 5.98 psi 2024-11-06T05:03:48Z- handleDepth()- Depth_Keller msg: 1730869428.23 epoch sec; seqNo:4234; depth 50.21 2024-11-06T05:03:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869428.24 epoch sec; seqNo:4234; 4061524.43 north; 594783.90 east; 36.69 lat; -121.94 lon; 50.21 depth; utmZone: 10 2024-11-06T05:03:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869425.02 2024-11-06T05:03:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:48Z- waiting for 1730869429.02, time is 1730869428.24 2024-11-06T05:03:48Z- AHRS_M2 msg: 1730869428.64 epoch sec; seqNo:3971 2024-11-06T05:03:48Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 5.98 psi 2024-11-06T05:03:48Z- handleDepth()- Depth_Keller msg: 1730869428.65 epoch sec; seqNo:4235; depth 50.18 2024-11-06T05:03:48Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869428.65 epoch sec; seqNo:4235; 4061524.81 north; 594783.90 east; 36.69 lat; -121.94 lon; 50.18 depth; utmZone: 10 2024-11-06T05:03:48Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:48Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869425.02 2024-11-06T05:03:48Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:48Z- waiting for 1730869429.02, time is 1730869428.65 2024-11-06T05:03:49Z- AHRS_M2 msg: 1730869429.02 epoch sec; seqNo:3972 2024-11-06T05:03:49Z- AHRS_M2 msg: 0.14 phi; 0.05 theta; 5.97 psi 2024-11-06T05:03:49Z- handleDepth()- Depth_Keller msg: 1730869429.02 epoch sec; seqNo:4236; depth 50.16 2024-11-06T05:03:49Z- handleDVL() - DVL msg epochMillisec: 1730869429.05 2024-11-06T05:03:49Z- handleDvl() - RDI_Pathfinder msg: 1730869429.05 sec; seqNo:1477; #beams:4 2024-11-06T05:03:49Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.76 , 28.90 , 30.89 , 37.31 2024-11-06T05:03:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.13 , -0.07 2024-11-06T05:03:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869429.06 epoch sec; seqNo:4236; 4061525.17 north; 594782.67 east; 36.69 lat; -121.94 lon; 50.16 depth; utmZone: 10 2024-11-06T05:03:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869425.02 2024-11-06T05:03:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:49Z- waiting for 1730869429.02, time is 1730869429.06 2024-11-06T05:03:49Z- LcmTrn::updateTrn() >>>> pose time 1730869429.06, meas time 1730869429.05 <<<< 2024-11-06T05:03:49Z- LcmTrn::updateTrn() equating pose 1730869429.05 and meas times 1730869429.05 2024-11-06T05:03:49Z- LcmTrn::updateTrn() motionUpdate first last update time: 1730869429.05 2024-11-06T05:03:49Z- TerrainNav::motionUpdate() - currEstimate = (4061525, 594782.67, 50.16) 2024-11-06T05:03:49Z- TerrainNav:: Windowed NIS is 0.0, in allowable region under 1.4. Keep on Trucking. 2024-11-06T05:03:49Z- TerrainNav:: SubcloudNIS is 0.37, in allowable region under 1.40. Keep on Trucking. 2024-11-06T05:03:49Z- TNavPF::Projecting Measurements 2024-11-06T05:03:49Z- TNavFilter:: ping # 0 currMeas.measStatus[0] = 1 2024-11-06T05:03:49Z- TNavFilter:: ping # 0 currMeas.measStatus[1] = 1 2024-11-06T05:03:49Z- TNavFilter:: ping # 0 currMeas.measStatus[2] = 1 2024-11-06T05:03:49Z- TNavFilter:: ping # 0 currMeas.measStatus[3] = 1 2024-11-06T05:03:49Z- TNavFilter::4 good beams. 2024-11-06T05:03:49Z- TNavPF::Measurements Projected, beam correspondences: 2024-11-06T05:03:49Z- TNavPF:: beamIndex[0] = 0 2024-11-06T05:03:49Z- TNavPF:: beamIndex[1] = 1 2024-11-06T05:03:49Z- TNavPF:: beamIndex[2] = 2 2024-11-06T05:03:49Z- TNavPF:: beamIndex[3] = 3 2024-11-06T05:03:49Z- TNavPF:: *** beamsVF.Ncols() = 4 *** 2024-11-06T05:03:49Z- TNavPF:: *** currMeas.numMeas = 4 *** 2024-11-06T05:03:49Z- TerrainMapOctree: (vehN, vehE ) = (4061525.17, 594782.67). 2024-11-06T05:03:49Z- TerrainMapOctree: Min Distance = 227.19. 2024-11-06T05:03:49Z- TerrainMapOctree: Using tile 5. 2024-11-06T05:03:49Z- Forcing Subcloud Comparison 2024-11-06T05:03:49Z- Weighting particles with subcloud comparison 2024-11-06T05:03:49Z- beam number: 0 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:49Z- meanExpectedMeasDiff: 1.709818 delta_rms_squared: 1.498630 alpha: 0.986830 2024-11-06T05:03:49Z- beam number: 1 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:49Z- meanExpectedMeasDiff: -2.059982 delta_rms_squared: 1.277178 alpha: 0.984582 2024-11-06T05:03:49Z- beam number: 2 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:49Z- meanExpectedMeasDiff: -0.687437 delta_rms_squared: 0.841544 alpha: 0.976786 2024-11-06T05:03:49Z- beam number: 3 num in subcloud: 10000 num not in subcloud: 0 2024-11-06T05:03:49Z- meanExpectedMeasDiff: 1.656404 delta_rms_squared: 0.727468 alpha: 0.973243 2024-11-06T05:03:49Z- Subcloud NIS: 0.392083 2024-11-06T05:03:49Z- TNavPF:: sumSquaredError = 0.000000 2024-11-06T05:03:49Z- TNavPF:: sumWeights = 0.181256 2024-11-06T05:03:49Z- TNavPF::Calculating NIS Matrices 2024-11-06T05:03:49Z- TNavPF::First Term of Map Covariance Matrix is 0.42 2024-11-06T05:03:49Z- TNavPF::Calculated NIS Value : 0.10 numBeams normalized NIS: 0.02 2024-11-06T05:03:49Z- TerrainNav::Resampling particle filter... 2024-11-06T05:03:49Z- TerrainNav::measUpdate - Measurement type 1 successfully incorporated from time = 1730869429.05 sec, ping # 0. 2024-11-06T05:03:49Z- variances([0], [2], and psi_berg) = 0.000000 0.000000 0.000000 2024-11-06T05:03:49Z- variances([0], [2], and psi_berg) = 1.114703 0.410771 0.000000 2024-11-06T05:03:49Z- publishEstimatesDecoder() - at 1730869429.05 sec 2024-11-06T05:03:49Z- LcmTrn::publishEstimatesDecoder() - reinits:0 filterstate:0 2024-11-06T05:03:49Z- LcmTrn::publishEstimatesDecoder() - MLE : 4061471.64 594786.28 58.39 2024-11-06T05:03:49Z- LcmTrn::publishEstimatesDecoder() - MMSE : 4061468.53 594786.20 59.29 lat/lon: 36.69 -121.94 2024-11-06T05:03:49Z- LcmTrn::publishEstimatesDecoder() - COVAR: 1.11 0.41 0.29 0.00 2024-11-06T05:03:49Z- AHRS_M2 msg: 1730869429.42 epoch sec; seqNo:3973 2024-11-06T05:03:49Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 5.97 psi 2024-11-06T05:03:49Z- handleDepth()- Depth_Keller msg: 1730869429.43 epoch sec; seqNo:4237; depth 50.14 2024-11-06T05:03:49Z- handleDVL() - DVL msg epochMillisec: 1730869429.43 2024-11-06T05:03:49Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:49Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:49Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:49Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:03:49Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:03:49Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:03:49Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:03:49Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:03:49Z- handleDvl() - RDI_Pathfinder msg: 1730869429.43 sec; seqNo:1478; #beams:0 2024-11-06T05:03:49Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:03:49Z- handleDvl() - RDI_Pathfinder msg: velocities 0.96 , -0.13 , -0.07 2024-11-06T05:03:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869429.46 epoch sec; seqNo:4237; 4061525.55 north; 594782.67 east; 36.69 lat; -121.94 lon; 50.14 depth; utmZone: 10 2024-11-06T05:03:49Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869429.05 2024-11-06T05:03:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:49Z- waiting for 1730869433.05, time is 1730869429.46 2024-11-06T05:03:49Z- AHRS_M2 msg: 1730869429.83 epoch sec; seqNo:3974 2024-11-06T05:03:49Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 5.97 psi 2024-11-06T05:03:49Z- handleDepth()- Depth_Keller msg: 1730869429.85 epoch sec; seqNo:4238; depth 50.12 2024-11-06T05:03:49Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869429.90 epoch sec; seqNo:4238; 4061525.93 north; 594782.67 east; 36.69 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T05:03:49Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:49Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869429.05 2024-11-06T05:03:49Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:49Z- waiting for 1730869433.05, time is 1730869429.90 2024-11-06T05:03:50Z- AHRS_M2 msg: 1730869430.23 epoch sec; seqNo:3975 2024-11-06T05:03:50Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 5.97 psi 2024-11-06T05:03:50Z- handleDepth()- Depth_Keller msg: 1730869430.24 epoch sec; seqNo:4239; depth 50.12 2024-11-06T05:03:50Z- handleDVL() - DVL msg epochMillisec: 1730869430.28 2024-11-06T05:03:50Z- handleDvl() - RDI_Pathfinder msg: 1730869430.28 sec; seqNo:1479; #beams:4 2024-11-06T05:03:50Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.60 , 28.90 , 30.58 , 36.85 2024-11-06T05:03:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.14 , -0.05 2024-11-06T05:03:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869430.29 epoch sec; seqNo:4239; 4061526.31 north; 594782.66 east; 36.69 lat; -121.94 lon; 50.12 depth; utmZone: 10 2024-11-06T05:03:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869429.05 2024-11-06T05:03:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:50Z- waiting for 1730869433.05, time is 1730869430.29 2024-11-06T05:03:50Z- AHRS_M2 msg: 1730869430.63 epoch sec; seqNo:3976 2024-11-06T05:03:50Z- AHRS_M2 msg: 0.13 phi; 0.04 theta; 5.97 psi 2024-11-06T05:03:50Z- handleDepth()- Depth_Keller msg: 1730869430.65 epoch sec; seqNo:4240; depth 50.10 2024-11-06T05:03:50Z- handleDVL() - DVL msg epochMillisec: 1730869430.65 2024-11-06T05:03:50Z- handleDVL() - platform_x_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:50Z- handleDVL() - platform_y_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:50Z- handleDVL() - platform_z_velocity_wrt_ground item not found in dvl msg 2024-11-06T05:03:50Z- handleDVL() - BottomVelocityFlag item not found in dvl msg 2024-11-06T05:03:50Z- handleDVL() - Beam1Range item not found in dvl msg 2024-11-06T05:03:50Z- handleDVL() - Beam2Range item not found in dvl msg 2024-11-06T05:03:50Z- handleDVL() - Beam3Range item not found in dvl msg 2024-11-06T05:03:50Z- handleDVL() - Beam4Range item not found in dvl msg 2024-11-06T05:03:50Z- handleDvl() - RDI_Pathfinder msg: 1730869430.65 sec; seqNo:1480; #beams:0 2024-11-06T05:03:50Z- handleDvl() - RDI_Pathfinder msg: ranges 0, 0.00 , 0.00 , 0.00 , 0.00 2024-11-06T05:03:50Z- handleDvl() - RDI_Pathfinder msg: velocities 0.99 , -0.14 , -0.05 2024-11-06T05:03:50Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869430.66 epoch sec; seqNo:4240; 4061526.69 north; 594782.66 east; 36.69 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T05:03:50Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:50Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869429.05 2024-11-06T05:03:50Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:50Z- waiting for 1730869433.05, time is 1730869430.66 2024-11-06T05:03:51Z- AHRS_M2 msg: 1730869431.04 epoch sec; seqNo:3977 2024-11-06T05:03:51Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 5.97 psi 2024-11-06T05:03:51Z- handleDepth()- Depth_Keller msg: 1730869431.05 epoch sec; seqNo:4241; depth 50.10 2024-11-06T05:03:51Z- handleDVL() - DVL msg epochMillisec: 1730869431.07 2024-11-06T05:03:51Z- handleDvl() - RDI_Pathfinder msg: 1730869431.07 sec; seqNo:1481; #beams:4 2024-11-06T05:03:51Z- handleDvl() - RDI_Pathfinder msg: ranges 1, 39.30 , 28.90 , 30.43 , 37.16 2024-11-06T05:03:51Z- handleDvl() - RDI_Pathfinder msg: velocities 0.97 , -0.14 , -0.01 2024-11-06T05:03:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869431.08 epoch sec; seqNo:4241; 4061527.07 north; 594782.65 east; 36.69 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T05:03:51Z- 4 msgs in 200 ms and 5 handle calls 2024-11-06T05:03:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869429.05 2024-11-06T05:03:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:51Z- waiting for 1730869433.05, time is 1730869431.08 2024-11-06T05:03:51Z- AHRS_M2 msg: 1730869431.44 epoch sec; seqNo:3978 2024-11-06T05:03:51Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 5.97 psi 2024-11-06T05:03:51Z- handleDepth()- Depth_Keller msg: 1730869431.45 epoch sec; seqNo:4242; depth 50.10 2024-11-06T05:03:51Z- handleNav() - DeadReckonUsingMultipleVelocitySources msg: 1730869431.48 epoch sec; seqNo:4242; 4061527.44 north; 594782.65 east; 36.69 lat; -121.94 lon; 50.10 depth; utmZone: 10 2024-11-06T05:03:51Z- 3 msgs in 200 ms and 4 handle calls 2024-11-06T05:03:51Z- LcmTrn::cycle() - lastUpdateTimestamp 1730869429.05 2024-11-06T05:03:51Z- LcmTrn::updateTrn() - updating TRN... 2024-11-06T05:03:51Z- waiting for 1730869433.05, time is 1730869431.48 2024-11-06T05:03:51Z- AHRS_M2 msg: 1730869431.85 epoch sec; seqNo:3979 2024-11-06T05:03:51Z- AHRS_M2 msg: 0.14 phi; 0.04 theta; 5.97 psi 2024-11-06T05:03:51Z- handleDepth()- Depth_Keller msg: 1730869431.86 epoch sec; seqNo:4243; depth 50.09